一种基于激光与惯导融合的足式机器人本体状态估计方法与流程

技术特征：

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算法，对所述激光扫描数据进行帧间匹配包括：

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

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

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

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

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

基于所述帧间匹配的结果，对经典icp的误差函数进行修改，通过计算得到点到线段距离的平方和，具体计算公式如下：

其中，cn为第n步中所有点到线段的对应关系，qn+1为下一次的位姿变换估计值，r(θn+1)和tn+1分别表示下一次的两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵，为投影点到参考平面的最近邻线段的垂直距离，为投影点在参考点集最近邻点；

通过求解所述误差函数的最小值，得到下一次的位姿变换估计值，具体公式如下：

其中，s^ref为通过参考扫描点集获取的一个参考表面集；

基于所述位姿变换估计值，得到足式机器人位姿变化，通过进一步计算可得到足式机器人本体的速度，进而得到增量运动变化。

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

选择搜索起始点，由当前扫描帧目标点的前一点的最近邻点确定；

基于所述搜索起始点，切换搜索方向，并建立上下游双向逆转机制；

在所述切换搜索方向之后，基于所述当前扫描帧目标点三角形三边关系建立估算最近距离，在未到达所述上下游边界时以该估算最近距离边界点为新边界，提前终止搜索；

基于所述搜索建立略过列表，允许匹配过程中跳过所述略过列表中重要性不足的点。

10.根据权利要求1所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态包括：

所述递归贝叶斯滤波器通过所述状态预测所输入的数据和所述状态校正与更新所输入的数据线性化得到过程雅可比；

基于所述过程雅可比，通过计算得到所述状态预测所输入的数据的协方差；

基于测量残差，修正所述状态预测所输入的数据；

基于增益矩阵，对所述测量残差进行缩放，得到校正后的置信均值；

在所述测量残差缩放之后，通过测量过程雅可比和所述状态预测所输入的数据的协方差，得到校正后状态的协方差；

基于所述校正后状态的协方差，得到所述足式机器人本体运动状态。