[发明专利]一种基于激光与惯导融合的足式机器人本体状态估计方法

权利要求书

1.一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述方法包括：

采集足式机器人本体的IMU数据以及激光数据；

对所述IMU数据与所述激光数据进行时间戳同步与预处理；

在所述时间戳同步与预处理之后，对所述IMU数据进行积分，得到积分数据，同时对所述激光数据进行帧间点线匹配，得到增量运动变化；

将所述积分数据作为状态预测输入至递归贝叶斯滤波器，且将所述得到的增量运动变化作为状态校正与更新输入至所述递归贝叶斯滤波器；

所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态。

2.根据权利要求1所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述采集足式机器人本体的IMU数据以及激光数据包括：

通过IMU传感器中的加速度计采集足式机器人本体的线性加速度；

通过IMU传感器中的陀螺仪采集足式机器人本体的角速度；

通过激光雷达采集足式机器人本体的激光数据。

3.根据权利要求1所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述对所述IMU数据与所述激光数据进行时间戳同步与预处理包括：

对所述IMU数据与所述激光数据进行时间戳同步，得到同步的IMU数据和激光数据；

基于所述IMU数据和所述激光数据，对所述IMU数据与所述激光数据进行预处理，其中，所述预处理包括将所述IMU数据和所述激光数据进行去噪，以及将所述IMU数据和所述激光数据进行坐标系转化。

4.根据权利要求3所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述将所述IMU数据进行坐标系转化包括：

基于所述加速度计所采集的线性加速度和所述陀螺仪所采集的角速度，在IMU坐标系中，使用适当的旋转和平移将所述IMU坐标系转化为足式机器人本体的坐标系。

5.根据权利要求1所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述对所述IMU数据进行积分，得到积分数据包括：

对所述IMU数据中的线性加速度进行一次积分和二次积分，分别得到速度和位置；

对所述IMU数据中的角速度进行一次积分得到角度，并将所述角度进行四元数转化，得到质心处由所述四元数表示的姿态信息。

6.根据权利要求1所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述对所述激光数据进行帧间点线匹配，得到增量运动变化包括：

基于点线ICP中的PLICP算法，对所述激光扫描数据进行帧间匹配；

基于所述帧间匹配的结果，通过计算得到增量运动变化。

7.根据权利要求6所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述基于点线ICP中的PLICP算法，对所述激光扫描数据进行帧间匹配包括：

在两点集的位姿变换估计下，将待匹配扫描点p_j集映射到参考扫描点集所在的坐标系中，并记为映射的转换关系具体计算公式如下：

其中，p_j为待匹配扫描点，为p_j映射到参考扫描点集坐标系下的点，Q_n＝(R_n，T_n)表示两组点集的位姿变换估计值，R_n和T_n分别表示两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵；

对于所述每个点均在所述参考扫描点集中找到距离最近的两个点的索引和

基于所述距离最近的两个点的索引和用一个元组进行表示点p_j到线段相匹配。