一种基于IMU的轮式机器人定位方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN108253961A(43)申请公布日2018.07.06(21)申请号201611241990.3(22)申请日2016.12.29(71)申请人北京雷动云合智能技术有限公司地址100085北京市海淀区安宁庄西路9号院29号楼807(72)发明人刘兴华胡兵兵郭煜玺(51)Int.Cl.G01C21/16(2006.01)G01C21/20(2006.01)权利要求书1页说明书2页附图1页(54)发明名称一种基于IMU的轮式机器人定位方法(57)摘要本发明提供了一种基于IMU的轮式机器人定位方法，属于机器人定位领域。其特征在于，通过在正常的SLAM算法中，建立一个包含N个姿态值att的滑动窗口，存储包含N个IMU数据、运动学参数滑动窗口用于回溯，使用IMU做纯惯性导航的方法来解决机器人“绑架”问题，即在发生碰撞、打滑等情况时把位置、姿态的更新算法切换到IMU的纯惯性导航算法上。本发明所述的方法解决了在一些窄视角、短距离测距模块应用中无法识别远距离物体、而近距场景在“绑架”时扫描匹配的结果也不是很准确、里程计和激光测距模块都无法使用的问题，而且该算法计算量不大，易于实现；同时，制造成本低，适合低成本机器人使用。CN108253961ACN108253961A权利要求书1/1页1.一种基于IMU的轮式机器人定位方法，其特征在于，包括以下步骤：步骤1：使用正常的SLAM算法融合里程计、测距模块、IMU数据；步骤2：建立一个包含N个姿态值att的滑动窗口，存储包含N个IMU数据、运动学参数滑动窗口用于回溯；步骤3：对步骤2中所述滑动窗口内的姿态值做奇异值分解，得到按降序排列的特征阈值wi{i＝1,2,3}；THTH步骤4：根据测试得到“绑架”时的特征阈值w2、w3，判断机器人是否被“绑架”：如果w2THTH＞w2，w3＞w3，则判定发生“绑架”，则执行步骤5；若没有发生“绑架”，则直接返回执行步骤1中正常的SLAM算法；步骤5：采用在当前时刻tdelay之前滑动窗口内的运动参数，开始回溯，采取加速追赶的策略，一个周期内回溯M个周期(M可以整除N)，即以tdelay之前时刻的值作为初值，切换到IMU的纯惯性导航算法上，则N/M个周期之后就可以追赶到机器人当前时刻，此时得到的位姿即是机器人当前正确的位姿；步骤6：完成上述步骤1～5的纯惯性导航回溯之后，返回步骤1进行正常的SLAM算法。2.根据权利要求1所述的机器人定位方法，其特征在于：所述步骤1中测距模块测距范围为0～5m，测距视角范围为0～180°。2CN108253961A说明书1/2页一种基于IMU的轮式机器人定位方法技术领域[0001]本发明属于机器人定位领域，尤其涉及一种基于IMU的轮式机器人定位方法。背景技术[0002]轮式机器人定位使用的传感器包括里程计、IMU、激光扫描仪模块等，在机器人行驶过程中可能发生碰撞、打滑、人为干扰等因素导致里程计无法反映机器人真实轨迹的情况，称为机器人“绑架”问题，纯里程计无法解决机器人“绑架”问题。现有技术在发生“绑架”时可以使用宽视角、长距离的激光扫描仪根据远距离的物体做SLAM算法中的扫描匹配来得到准确的位姿，避开里程计不准确的问题。但是在一些窄视角、短距离激光扫描仪应用中，无法识别远距离物体，而近距场景在“绑架”时扫描匹配的结果也是不准确的，此时里程计和激光扫描仪都无法使用。发明内容[0003]本发明针对上述技术存在的缺陷，提出了一种基于IMU的轮式机器人定位方法，使用IMU做纯捷联惯性导航的方法来解决“绑架”问题，即在发生碰撞、打滑等情况时把位置、姿态的更新算法切换到IMU的纯惯性导航算法上。[0004]为实现上述目的，本发明是通过如下方案实现的，一种基于IMU的轮式机器人定位方法，其特征在于，包括以下步骤：[0005]步骤1：使用正常的SLAM算法融合里程计、测距模块、IMU数据；[0006]步骤2：建立一个包含N个姿态值att的滑动窗口，存储包含N个IMU数据、运动学参数滑动窗口用于回溯；[0007]步骤3：对步骤2中所述滑动窗口内的姿态值做奇异值分解，得到按降序排列的特征阈值wi{i＝1,2,3}；THTH[0008]步骤4：根据测试得到“绑架”时的特征阈值w2、w3，判断机器人是否被“绑架”：如THTH果w2＞w2，w3＞w3，则判定发生“绑架”，则执行步骤5；若没有发生“绑架”，则直接返回执行步骤1中正常的SLAM算法；[0009]步骤5：由于判断会发生一个延时tdelay，即判断发生绑架时已经发生绑架tdelay时间了，则纯惯性导航需要回溯到绑架发生的时刻(tdelay之前)，采用在当前时刻tdelay之前滑动窗口内的运动参数，开始回溯，采取加速