CN108253961A - 一种基于imu的轮式机器人定位方法 - Google Patents

Abstract

本发明提供了一种基于IMU的轮式机器人定位方法，属于机器人定位领域。其特征在于，通过在正常的SLAM算法中，建立一个包含N个姿态值att的滑动窗口，存储包含N个IMU数据、运动学参数滑动窗口用于回溯，使用IMU做纯惯性导航的方法来解决机器人“绑架”问题，即在发生碰撞、打滑等情况时把位置、姿态的更新算法切换到IMU的纯惯性导航算法上。本发明所述的方法解决了在一些窄视角、短距离测距模块应用中无法识别远距离物体、而近距场景在“绑架”时扫描匹配的结果也不是很准确、里程计和激光测距模块都无法使用的问题，而且该算法计算量不大，易于实现；同时，制造成本低，适合低成本机器人使用。

Description

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

技术领域

本发明属于机器人定位领域，尤其涉及一种基于IMU的轮式机器人定位方法。

背景技术

轮式机器人定位使用的传感器包括里程计、IMU、激光扫描仪模块等，在机器人行驶过程中可能发生碰撞、打滑、人为干扰等因素导致里程计无法反映机器人真实轨迹的情况，称为机器人“绑架”问题，纯里程计无法解决机器人“绑架”问题。现有技术在发生“绑架”时可以使用宽视角、长距离的激光扫描仪根据远距离的物体做SLAM算法中的扫描匹配来得到准确的位姿，避开里程计不准确的问题。但是在一些窄视角、短距离激光扫描仪应用中，无法识别远距离物体，而近距场景在“绑架”时扫描匹配的结果也是不准确的，此时里程计和激光扫描仪都无法使用。

发明内容

本发明针对上述技术存在的缺陷，提出了一种基于IMU的轮式机器人定位方法，使用IMU做纯捷联惯性导航的方法来解决“绑架”问题，即在发生碰撞、打滑等情况时把位置、姿态的更新算法切换到IMU的纯惯性导航算法上。

为实现上述目的，本发明是通过如下方案实现的，一种基于IMU的轮式机器人定位方法，其特征在于，包括以下步骤：

步骤1：使用正常的SLAM算法融合里程计、测距模块、IMU数据；

步骤2：建立一个包含N个姿态值att的滑动窗口，存储包含N个IMU数据、运动学参数滑动窗口用于回溯；

步骤3：对步骤2中所述滑动窗口内的姿态值做奇异值分解，得到按降序排列的特征阈值w_i{i＝1,2,3}；

步骤4：根据测试得到“绑架”时的特征阈值w₂ ^TH、w₃ ^TH，判断机器人是否被“绑架”：如果w₂＞w₂ ^TH，w₃＞w₃ ^TH，则判定发生“绑架”，则执行步骤5；若没有发生“绑架”，则直接返回执行步骤1中正常的SLAM算法；

步骤5：由于判断会发生一个延时t_delay，即判断发生绑架时已经发生绑架t_delay时间了，则纯惯性导航需要回溯到绑架发生的时刻(t_delay之前)，采用在当前时刻t_delay之前滑动窗口内的运动参数，开始回溯，采取加速追赶的策略，一个周期内回溯M个周期(M可以整除N)，即以t_delay之前时刻的值作为初值，切换到IMU的纯惯性导航算法上，则N/M个周期之后就可以追赶到机器人当前时刻，此时得到的位姿即是机器人当前正确的位姿。

步骤6：完成上述步骤1～5的纯惯性导航回溯之后，返回步骤1进行正常的SLAM算法。

所述步骤1中测距模块测距范围为0～5m，测距视角范围为0～180°。

本发明具有如下有益效果：解决了机器人在一些窄视角、短距离激光扫描仪应用正无法识别远距离物体，而近距场景在“绑架”时扫描匹配的结果也不是很准确，里程计和激光扫描仪都无法使用的问题，而且算法计算量不大，易于实现；同时，制造成本低，适合低成本机器人使用。

附图说明

图1为本发明所述的基于IMU的轮式机器人定位方法的流程图。

具体实施方式

下面结合附图对本发明做进一步的说明。

在机器人研发平台上采用一个MPU6050IMU，同时配置了里程计，一个110°视角、测距距离为2m的2D激光扫描仪，IMU和里程计做航位推算，更新周期是10ms。

实施例1：如附图1所示，一种基于IMU的轮式机器人定位方法，包括以下步骤：

步骤1：使用正常的SLAM算法融合里程计、2D激光扫描仪、IMU数据；

步骤2：建立一个包含30个姿态值att的滑动窗口，存储30个(0.3s)IMU数据、运动学参数的滑动窗口用于回溯；

步骤3：对步骤2中滑动窗口内的姿态值做奇异值分解，得到按降序排列的特征阈值w_i{i＝1,2,3}；

步骤4：根据测试得到“绑架”时的特征阈值w₂ ^TH、w₃ ^TH，判断机器人是否被“绑架”：如果w₂＞w₂ ^TH，w₃＞w₃ ^TH，则判定发生“绑架”，则继续执行步骤5；若没有发生“绑架”，则直接返回继续执行步骤1中正常的SLAM算法；

步骤5：由于判断会发生一个延时t_delay，即判断发生绑架时已经发生绑架t_delay时间了，则纯惯性导航需要回溯到绑架发生的时刻(t_delay之前)，采用在当前时刻t_delay(0.2s)之前的滑动窗口的运动学参数，开始回溯，采取加速追赶的策略，一个周期内回溯3个周期(3可以整除30)，即以t_delay之前时刻的值作为初值，切换到IMU的纯惯性导航算法，则10个周期之后就可以追赶到机器人当前时刻，此时得到的位姿即是正确的位姿。

步骤6：完成上述步骤1～5的纯惯性导航回溯之后，接着返回步骤1进行正常的SLAM算法。

前述实施例和优点仅是示例性的，并不应被理解为限制本公开。本发明可容易地应用于其它类型的设备。此外，本公开的示例性实施例的描述是解释性的，并不限制权利要求的范围，许多的替换、修改和变化对于本领域技术人员来说是明显的。

Claims (2)

1.一种基于IMU的轮式机器人定位方法，其特征在于，包括以下步骤：

步骤1：使用正常的SLAM算法融合里程计、测距模块、IMU数据；

步骤2：建立一个包含N个姿态值att的滑动窗口，存储包含N个IMU数据、运动学参数滑动窗口用于回溯；

步骤3：对步骤2中所述滑动窗口内的姿态值做奇异值分解，得到按降序排列的特征阈值w_i{i＝1,2,3}；

步骤4：根据测试得到“绑架”时的特征阈值w₂ ^TH、w₃ ^TH，判断机器人是否被“绑架”：如果w₂＞w₂ ^TH，w₃＞w₃ ^TH，则判定发生“绑架”，则执行步骤5；若没有发生“绑架”，则直接返回执行步骤1中正常的SLAM算法；

步骤5：采用在当前时刻t_delay之前滑动窗口内的运动参数，开始回溯，采取加速追赶的策略，一个周期内回溯M个周期(M可以整除N)，即以t_delay之前时刻的值作为初值，切换到IMU的纯惯性导航算法上，则N/M个周期之后就可以追赶到机器人当前时刻，此时得到的位姿即是机器人当前正确的位姿；

步骤6：完成上述步骤1～5的纯惯性导航回溯之后，返回步骤1进行正常的SLAM算法。

2.根据权利要求1所述的机器人定位方法，其特征在于：所述步骤1中测距模块测距范围为0～5m，测距视角范围为0～180°。