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

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

Info

Publication number
CN110954100A
CN110954100A CN201911424943.6A CN201911424943A CN110954100A CN 110954100 A CN110954100 A CN 110954100A CN 201911424943 A CN201911424943 A CN 201911424943A CN 110954100 A CN110954100 A CN 110954100A
Authority
CN
China
Prior art keywords
data
laser
state
point
imu
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201911424943.6A
Other languages
English (en)
Inventor
苏泽荣
周雪峰
唐观荣
吴鸿敏
鄢武
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong Institute of Intelligent Manufacturing
Original Assignee
Guangdong Institute of Intelligent Manufacturing
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangdong Institute of Intelligent Manufacturing filed Critical Guangdong Institute of Intelligent Manufacturing
Priority to CN201911424943.6A priority Critical patent/CN110954100A/zh
Publication of CN110954100A publication Critical patent/CN110954100A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种基于激光与惯导融合的足式机器人本体状态估计方法，所述方法包括：采集足式机器人本体的IMU数据以及激光数据；对所述IMU数据与所述激光数据进行时间戳同步与预处理；在所述时间戳同步与预处理之后，对所述IMU数据进行积分，得到积分数据，同时对所述激光数据进行激光扫描和帧间点线匹配，得到增量运动变化；将所述积分数据作为状态预测输入至递归贝叶斯滤波器，且将所述得到的增量运动变化作为状态校正与更新输入至所述递归贝叶斯滤波器；所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态。在本发明实施中，提高状态估计的可靠性。

Description

一种基于激光与惯导融合的足式机器人本体状态估计方法
技术领域
本发明涉及一种足式机器人本地运动状态的技术领域，尤其涉及一种基于激光与惯导融合的足式机器人本体状态估计方法。
背景技术
移动机器人需要对其状态进行良好的估计，才能在结构化和非结构化环境中进行闭环控制。许多现有的算法都依赖来自多个传感器的数据融合以计算这些估计值。该技术领域常用本体感应传感器的测量信息，如加速度、机器人内部的电机速度、关节角度，以及来自外部特征的测量信息，如光强度、距离测量、声音振幅等进行融合，然而这些数据来源并不总是可靠的。轮式机器人通常使用IMU以及电机编码器在与地面保持平稳可靠接触下能较好的进行里程和状态估计，而腿式机器人通过间歇性的脚底接触与环境接触交互，对IMU和编码器测量中引入了额外的噪声，足式机器人的里程状态通常采用卡尔曼滤波器，需要仔细调参，单靠IMU和关节传感器进行位置常发生飘逸。而传统足式机器人基于本体IMU容易受到噪声的影响，同时纯靠激光传感器信息容易受遮挡出现丢帧，无法匹配进行估计的现象。
发明内容
本发明的目的在于克服现有技术的不足，本发明提供了一种基于激光与惯导融合的足式机器人本体状态估计方法，融合内部感应传感器和外部测量传感器信息，避免依赖单一数据来源，采用点线匹配算法的二次收敛特性加速算法运行，利用扩展卡尔曼滤波对激光里程计和IMU里程计数据进行融合，最终得到本机器人本体状态的可靠估计。
为了解决上述技术问题，本发明实施例提供了一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述方法包括：
采集足式机器人本体的IMU数据以及激光数据；
对所述IMU数据与所述激光数据进行时间戳同步与预处理；
在所述时间戳同步与预处理之后，对所述IMU数据进行积分，得到积分数据，同时对所述激光数据进行帧间点线匹配，得到增量运动变化；
将所述积分数据作为状态预测输入至递归贝叶斯滤波器，且将所述得到的增量运动变化作为状态校正与更新输入至所述递归贝叶斯滤波器；
所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态。
可选的，其特征在于，所述采集足式机器人本体的IMU数据以及激光数据包括：
通过IMU传感器中的加速度计采集足式机器人本体的线性加速度；
通过IMU传感器中的陀螺仪采集足式机器人本体的角速度；
通过激光雷达采集足式机器人本体的激光数据。
可选的，所述对所述IMU数据与所述激光数据进行时间戳同步与预处理包括：
对所述IMU数据与所述激光数据进行时间戳同步，得到同步的IMU数据和激光数据；
基于所述IMU数据和所述激光数据，对所述IMU数据与所述激光数据进行预处理，其中，所述预处理包括将所述IMU数据和所述激光数据进行去噪，以及将所述IMU数据和所述激光数据进行坐标系转化。
可选的，所述将所述IMU数据进行坐标系转化包括：
基于所述加速度计所采集的线性加速度和所述陀螺仪所采集的角速度，在IMU坐标系中，使用适当的旋转和平移将所述IMU坐标系转化为足式机器人本体的坐标系。
可选的，所述对所述IMU数据进行积分，得到积分数据包括：
对所述IMU数据中的线性加速度进行一次积分和二次积分，分别得到速度和位置；
对所述IMU数据中的角速度进行一次积分得到角度，并将所述角度进行四元数转化，得到质心处由所述四元数表示的姿态信息。
可选的，所述对所述激光数据进行帧间点线匹配，得到增量运动变化包括：
基于点线ICP中的PLICP算法，对所述激光扫描数据进行帧间匹配；
基于所述帧间匹配的结果，通过计算得到增量运动变化。
可选的，所述基于点线ICP中的PLICP算法，对所述激光扫描数据进行帧间匹配包括：
在两点集的位姿变换估计下，将待匹配扫描点pj集映射到参考扫描点集所在的坐标系中，并记为
Figure BDA0002347637620000031
映射的转换关系具体计算公式如下：
Figure BDA0002347637620000032
其中，pj为待匹配扫描点，
Figure BDA0002347637620000033
为pj映射到参考扫描点集坐标系下的点，Qn＝(Rn，Tn)表示两组点集的位姿变换估计值，Rn和Tn分别表示两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵；
对于所述每个点
Figure BDA0002347637620000034
均在所述参考扫描点集中找到距离最近的两个点的索引
Figure BDA0002347637620000035
和
Figure BDA0002347637620000036
基于所述距离最近的两个点的索引
Figure BDA0002347637620000037
和
Figure BDA0002347637620000038
用一个元组
Figure BDA0002347637620000039
进行表示点pj到线段
Figure BDA00023476376200000310
相匹配。
可选的，所述基于所述帧间匹配的结果，通过计算得到增量运动变化包括：
基于所述帧间匹配的结果，对经典ICP的误差函数进行修改，通过计算得到点到线段距离的平方和，具体计算公式如下：
Figure BDA00023476376200000311
其中，Cn为第n步中所有点到线段的对应关系，Qn+1为下一次的位姿变换估计值，R(θn+1)和Tn+1分别表示下一次的两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵，
Figure BDA00023476376200000312
为投影点到参考平面的最近邻线段的垂直距离，
Figure BDA00023476376200000313
为投影点在参考点集最近邻点；
通过求解所述误差函数的最小值，得到下一次的位姿变换估计值，具体公式如下：
Figure BDA0002347637620000041
其中，Sref为通过参考扫描点集获取的一个参考表面集；
基于所述位姿变换估计值，得到足式机器人位姿变化，通过进一步计算可得到足式机器人本体的速度，进而得到增量运动变化。
可选的，所述基于点线ICP中的PLICP算法，对所述激光扫描数据进行帧间匹配还包括快速搜索对应点对，其中，所述快速搜索对应点对包括：
选择搜索起始点，由当前扫描帧目标点的前一点的最近邻点确定；
基于所述搜索起始点，切换搜索方向，并建立上下游双向逆转机制；
在所述切换搜索方向之后，基于所述当前扫描帧目标点三角形三边关系建立估算最近距离，在未到达所述上下游边界时以该估算最近距离边界点为新边界，提前终止搜索；
基于所述搜索建立略过列表，允许匹配过程中跳过所述略过列表中重要性不足的点。
可选的，所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态包括：
所述递归贝叶斯滤波器通过所述状态预测所输入的数据和所述状态校正与更新所输入的数据线性化得到过程雅可比；
基于所述过程雅可比，通过计算得到所述状态预测所输入的数据的协方差；
基于测量残差，修正所述状态预测所输入的数据；
基于增益矩阵，对所述测量残差进行缩放，得到校正后的置信均值；
在所述测量残差缩放之后，通过测量过程雅可比和所述状态预测所输入的数据的协方差，得到校正后状态的协方差；
基于所述校正后状态的协方差，得到所述足式机器人本体运动状态。
在本发明实施中，针对传统足式机器人基于本体IMU估计易受到噪声的影响，同时纯靠激光传感器信息易受遮挡出现丢帧无法匹配进行估计的现象，提出基于扩展卡尔曼滤波器的信息融合方法，实现了状态估计，提高了状态估计的可靠性；针对传统足式机器人激光扫描数据匹配精度和效率不高的问题，提出基于点线匹配的帧间增量运动估计方法，算法假设激光扫描在环境中呈现多线段现象比常用算法更贴近实际情况，使得减少帧间误匹配次数，同时也提高了匹配效率和精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明实施例中的一种基于激光与惯导融合的足式机器人本体状态估计方法的流程示意图；
图2是本发明实施例中的PLICP算法原理图；
图3是本发明实施例中的PLICP算法的迭代示意图；
图4是本发明实施例中的混合快速搜索关联点策略示意图a；
图5是本发明实施例中的混合快速搜索关联点策略示意图b；
图6是本发明实施例中的混合快速搜索关联点策略示意图c；
图7是本发明实施例中的混合快速搜索关联点策略示意图d。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
实施例
请参阅图1，图1是本发明实施例中的一种基于激光与惯导融合的足式机器人本体状态估计方法的流程示意图。
如图1所示，一种基于激光与惯导融合的足式机器人本体运动状态方法，所述方法包括：
S11：采集足式机器人本体的IMU数据以及激光数据；
在本发明具体实施过程中，所述采集足式机器人本体的IMU数据以及激光数据包括：通过IMU传感器中的加速度计采集足式机器人本体的线性加速度；通过IMU传感器中的陀螺仪采集足式机器人本体的角速度；通过激光雷达采集足式机器人本体的激光数据。
S12：对所述IMU数据与所述激光数据进行时间戳同步与预处理；
在本发明具体实施过程中，所述对所述IMU数据与所述激光数据进行时间戳同步与预处理包括：对所述IMU数据与所述激光数据进行时间戳同步，得到同步的IMU数据和激光数据；基于所述IMU数据和所述激光数据，对所述IMU数据与所述激光数据进行预处理，其中，所述预处理包括将所述IMU数据和所述激光数据进行去噪，以及将所述IMU数据和所述激光数据进行坐标系转化。
具体的，所述将所述IMU数据进行坐标系转化包括：基于所述加速度计所采集的线性加速度和所述陀螺仪所采集的角速度，在IMU坐标系中，使用适当的旋转和平移将所述IMU坐标系转化为足式机器人本体的坐标系。
S13：在所述时间戳同步与预处理之后，对所述IMU数据进行积分，得到积分数据，同时对所述激光数据进行帧间点线匹配，得到增量运动变化；
在本发明具体实施过程中，所述对所述IMU数据进行积分，得到积分数据包括：对所述IMU数据中的线性加速度进行一次积分和二次积分，分别得到速度和位置；对所述IMU数据中的角速度进行一次积分得到角度，并将所述角度进行四元数转化，得到质心处由所述四元数表示的姿态信息。
在本发明具体实施过程中，所述对所述激光数据进行帧间点线匹配，得到增量运动变化包括：基于点线ICP中的PLICP算法，对所述激光扫描数据进行帧间匹配；基于所述帧间匹配的结果，通过计算得到增量运动变化。
具体的，给定一组参考扫描点集Pt-1，该点集中点表示为pi，当前待匹配扫描点集Pt，该点集中点表示为pj，以及两组点集位姿变换的估计值Q，通过参考扫描点集可获取一个参考表面集，记为Sref，PLICP基本假设是匹配点集的参考表面是多边形，也即自然环境是局部线性，这一点在一般室内环境中均能够得到满足；故通常可设定阈值筛选近邻的两个点将扫描点集组成一个多边形。结合附图2所示，附图2示出本发明实施例中的PLICP算法原理图，虚线圆点和多折线分别为参考点集和参考表面集，实心圆点pj为待匹配点集，线段表示点到线的对应关系。记n为算法的迭代步数，则算法的流程具体如下：
在两点集的位姿变换估计Qn＝(Rn，Tn)下，将待匹配扫描点pj集映射到参考扫描点集所在的坐标系中；将pj映射到参考点集坐标系下的点记为
Figure BDA0002347637620000071
映射的转换关系具体计算公式如下：
Figure BDA0002347637620000072
其中，pj为待匹配扫描点，
Figure BDA0002347637620000073
为pj映射到参考扫描点集坐标系下的点，Qn＝(Rn，Tn)表示两组点集的位姿变换估计值，Rn和Tn分别表示两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵；
对于所述每个点
Figure BDA0002347637620000074
均在所述参考扫描点集中找到距离最近的两个点的索引
Figure BDA0002347637620000075
和
Figure BDA0002347637620000076
基于所述距离最近的两个点的索引
Figure BDA0002347637620000077
和
Figure BDA0002347637620000078
用一个元组
Figure BDA0002347637620000079
进行表示点pj到线段
Figure BDA00023476376200000710
相匹配。
基于所述帧间匹配的结果，对经典ICP的误差函数进行修改，通过计算得到点到线段距离的平方和，具体计算公式如下：
Figure BDA00023476376200000711
其中，Cn为第n步中所有点到线段的对应关系，Qn+1为下一次的位姿变换估计值，R(θn+1)和Tn+1分别表示下一次的两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵，
Figure BDA00023476376200000712
为投影点到参考平面的最近邻线段的垂直距离，
Figure BDA00023476376200000713
为投影点在参考点集最近邻点；
通过求解所述误差函数的最小值，得到下一次的位姿变换估计值，具体公式如下：
Figure BDA0002347637620000081
其中，Sref为通过参考扫描点集获取的一个参考表面集；
基于所述位姿变换估计值，得到足式机器人位姿变化，通过进一步计算可得到足式机器人本体的速度，进而得到增量运动变化。需要说明的是，重复以上步骤进行n次迭代，直到算法收敛或循环结束，完成两幅扫描数据的匹配过程，结合附图3所示，图3示出本发明实施例中的PLICP算法的迭代示意图。
另外，具体的，所述基于点线ICP中的PLICP算法，对所述激光扫描数据进行帧间匹配还包括快速搜索对应点对的策略，并且适用于传统ICP算法的对应点对搜索。原始的搜索策略是遍历序列中所有样本点，优化算法改进了搜索范围，在每个参考帧坐标系中的目标搜索点的搜索区间，结合附图4所示，附图4示出本发明实施例中的混合快速搜索关联点策略示意图a，往极坐标的中心视角方向分别左右增加一定角度范围
Figure BDA0002347637620000082
Figure BDA0002347637620000083
当对帧间转换关系的最大值Qmax＝(R(θ)max，Tmax)具有先验知识的情况下，
Figure BDA0002347637620000084
的上限可通过下式求得：
Figure BDA0002347637620000085
可见，原始的搜索策略搜索空间冗余，且不一定存在转换关系的先验知识，基于此，混合以下几种策略共同作用进行改善搜索速度，具体如下：
(1)搜索起始点选择；结合附图5所示，附图5示出本发明实施例中的混合快速搜索关联点策略示意图b；起始搜索索引由当前扫描帧目标点pj的前一点pj-1的最近邻点
Figure BDA0002347637620000086
确定，邻近点的最近邻点符合对应关系的关联可能性较大；
(2)切换搜索方向；结合附图5所示，附图5示出本发明实施例中的混合快速搜索关联点策略示意图b；建立上下游双向逆转机制，以增量α单向搜索，距离差量符号为方向逆变标识，直至上下两游边界；
(3)提前终止准则；结合附图6所示，附图6示出本发明实施例中的混合快速搜索关联点策略示意图c；该准则主要改善固定搜索区域导致遍历冗余数据的资源消耗问题。基于当前扫描帧目标点直接三角形三边关系建立估算最近距离，在未到达上下两游边界以该估算最近距离边界点为新边界，提前终止搜索；
(4)建立略过列表；结合附图7所示，附图7示出本发明实施例中的混合快速搜索关联点策略示意图d；允许匹配过程中跳过列表中重要性不足的点，对每帧只需构造一次，在整个迭代过程中可复用，实际执行中，该列表占用计算耗时少于执行时间的1％；记参考帧pj-1中每个点pi，在属于i′＞i上游索引的点，记较小(大)的读数为ρi′，对上下游保存大小数：
up_smaller[i]＝min{i′＞i|ρi′＜ρi}；
up_bigger[i]＝min{i′＞i|pi′＞pi}。
S14：将所述积分数据作为状态预测输入至递归贝叶斯滤波器，且将所述得到的增量运动变化作为状态校正与更新输入至所述递归贝叶斯滤波器；
S15：所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态。
在本发明具体实施过程中，所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态包括：所述递归贝叶斯滤波器通过所述状态预测所输入的数据和所述状态校正与更新所输入的数据线性化得到过程雅可比；基于所述过程雅可比，通过计算得到所述状态预测所输入的数据的协方差；基于测量残差，修正所述状态预测所输入的数据；基于增益矩阵，对所述测量残差进行缩放，得到校正后的置信均值；在所述测量残差缩放之后，通过测量过程雅可比和所述状态预测所输入的数据的协方差，得到校正后状态的协方差；基于所述校正后状态的协方差，得到所述足式机器人本体运动状态。
在本发明实施中，针对传统足式机器人基于本体IMU估计易受到噪声的影响，同时纯靠激光传感器信息易受遮挡出现丢帧无法匹配进行估计的现象，提出基于扩展卡尔曼滤波器的信息融合方法，实现了状态估计，提高了状态估计的可靠性；针对传统足式机器人激光扫描数据匹配精度和效率不高的问题，提出基于点线匹配的帧间增量运动估计方法，算法假设激光扫描在环境中呈现多线段现象比常用算法更贴近实际情况，使得减少帧间误匹配次数，同时也提高了匹配效率和精度。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁盘或光盘等。
另外，以上对本发明实施例所提供的一种基于激光与惯导融合的足式机器人本体状态估计方法进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

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集映射到参考扫描点集所在的坐标系中，并记为
Figure FDA0002347637610000021
映射的转换关系具体计算公式如下：
Figure FDA0002347637610000022
其中，pj为待匹配扫描点，
Figure FDA0002347637610000031
为pj映射到参考扫描点集坐标系下的点，Qn＝(Rn，Tn)表示两组点集的位姿变换估计值，Rn和Tn分别表示两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵；
对于所述每个点
Figure FDA0002347637610000032
均在所述参考扫描点集中找到距离最近的两个点的索引
Figure FDA0002347637610000033
和
Figure FDA0002347637610000034
基于所述距离最近的两个点的索引
Figure FDA0002347637610000035
和
Figure FDA0002347637610000036
用一个元组
Figure FDA0002347637610000037
进行表示点pj到线段
Figure FDA0002347637610000038
相匹配。
8.根据权利要求6所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述基于所述帧间匹配的结果，通过计算得到增量运动变化包括：
基于所述帧间匹配的结果，对经典ICP的误差函数进行修改，通过计算得到点到线段距离的平方和，具体计算公式如下：
Figure FDA0002347637610000039
其中，Cn为第n步中所有点到线段的对应关系，Qn+1为下一次的位姿变换估计值，R(θn+1)和Tn+1分别表示下一次的两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵，
Figure FDA00023476376100000310
为投影点到参考平面的最近邻线段的垂直距离，
Figure FDA00023476376100000311
为投影点在参考点集最近邻点；
通过求解所述误差函数的最小值，得到下一次的位姿变换估计值，具体公式如下：
Figure FDA00023476376100000312
其中，Sref为通过参考扫描点集获取的一个参考表面集；
基于所述位姿变换估计值，得到足式机器人位姿变化，通过进一步计算可得到足式机器人本体的速度，进而得到增量运动变化。
9.根据权利要求1所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述基于点线ICP中的PLICP算法，对所述激光扫描数据进行帧间匹配还包括快速搜索对应点对，其中，所述快速搜索对应点对包括：
选择搜索起始点，由当前扫描帧目标点的前一点的最近邻点确定；
基于所述搜索起始点，切换搜索方向，并建立上下游双向逆转机制；
在所述切换搜索方向之后，基于所述当前扫描帧目标点三角形三边关系建立估算最近距离，在未到达所述上下游边界时以该估算最近距离边界点为新边界，提前终止搜索；
基于所述搜索建立略过列表，允许匹配过程中跳过所述略过列表中重要性不足的点。
10.根据权利要求1所述的一种基于激光与惯导融合的足式机器人本体状态估计方法，其特征在于，所述递归贝叶斯滤波器通过对比所述状态预测所输入的数据与所述状态校正与更新所输入的数据来进行误差测量，得到所述足式机器人本体运动状态包括：
所述递归贝叶斯滤波器通过所述状态预测所输入的数据和所述状态校正与更新所输入的数据线性化得到过程雅可比；
基于所述过程雅可比，通过计算得到所述状态预测所输入的数据的协方差；
基于测量残差，修正所述状态预测所输入的数据；
基于增益矩阵，对所述测量残差进行缩放，得到校正后的置信均值；
在所述测量残差缩放之后，通过测量过程雅可比和所述状态预测所输入的数据的协方差，得到校正后状态的协方差；
基于所述校正后状态的协方差，得到所述足式机器人本体运动状态。
CN201911424943.6A 2019-12-30 2019-12-30 一种基于激光与惯导融合的足式机器人本体状态估计方法 Pending CN110954100A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911424943.6A CN110954100A (zh) 2019-12-30 2019-12-30 一种基于激光与惯导融合的足式机器人本体状态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911424943.6A CN110954100A (zh) 2019-12-30 2019-12-30 一种基于激光与惯导融合的足式机器人本体状态估计方法

Publications (1)

Publication Number Publication Date
CN110954100A true CN110954100A (zh) 2020-04-03

Family

ID=69985402

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911424943.6A Pending CN110954100A (zh) 2019-12-30 2019-12-30 一种基于激光与惯导融合的足式机器人本体状态估计方法

Country Status (1)

Country Link
CN (1) CN110954100A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111638530A (zh) * 2020-05-27 2020-09-08 广州蓝胖子机器人有限公司 一种叉车定位的方法、叉车及计算机可读存储介质
CN111678515A (zh) * 2020-04-28 2020-09-18 北京三快在线科技有限公司 设备状态估计方法、装置、电子设备及存储介质
CN112269187A (zh) * 2020-09-28 2021-01-26 广州视源电子科技股份有限公司 机器人状态检测方法、装置及设备
CN113566737A (zh) * 2021-07-28 2021-10-29 赤湾集装箱码头有限公司 岸边起重机船体离梆监测方法及其岸边起重机

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1563889A (zh) * 2004-03-26 2005-01-12 清华大学 激光跟踪惯性组合测量系统及其测量方法
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN106525053A (zh) * 2016-12-28 2017-03-22 清研华宇智能机器人（天津）有限责任公司 一种基于多传感器融合的移动机器人室内定位方法
CN106918830A (zh) * 2017-03-23 2017-07-04 安科机器人有限公司 一种基于多导航模块的定位方法及移动机器人
CN108955679A (zh) * 2018-08-16 2018-12-07 电子科技大学 一种变电站智能巡检机器人高精度定位方法
CN109341705A (zh) * 2018-10-16 2019-02-15 北京工业大学 智能探测机器人同时定位与地图构建系统
CN110260867A (zh) * 2019-07-29 2019-09-20 浙江大华技术股份有限公司 一种机器人导航中位姿确定、纠正的方法、设备及装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1563889A (zh) * 2004-03-26 2005-01-12 清华大学 激光跟踪惯性组合测量系统及其测量方法
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN106525053A (zh) * 2016-12-28 2017-03-22 清研华宇智能机器人（天津）有限责任公司 一种基于多传感器融合的移动机器人室内定位方法
CN106918830A (zh) * 2017-03-23 2017-07-04 安科机器人有限公司 一种基于多导航模块的定位方法及移动机器人
CN108955679A (zh) * 2018-08-16 2018-12-07 电子科技大学 一种变电站智能巡检机器人高精度定位方法
CN109341705A (zh) * 2018-10-16 2019-02-15 北京工业大学 智能探测机器人同时定位与地图构建系统
CN110260867A (zh) * 2019-07-29 2019-09-20 浙江大华技术股份有限公司 一种机器人导航中位姿确定、纠正的方法、设备及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
ANDREA CENSI: "An ICP variant using a point-to-line metric", 《2008 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 *
顾文华等: "基于ICP匹配算法的室内移动机器人定位", 《华中科技大学学报（自然科学版）》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111678515A (zh) * 2020-04-28 2020-09-18 北京三快在线科技有限公司 设备状态估计方法、装置、电子设备及存储介质
CN111638530A (zh) * 2020-05-27 2020-09-08 广州蓝胖子机器人有限公司 一种叉车定位的方法、叉车及计算机可读存储介质
CN111638530B (zh) * 2020-05-27 2023-09-19 广州蓝胖子移动科技有限公司 一种叉车定位的方法、叉车及计算机可读存储介质
CN112269187A (zh) * 2020-09-28 2021-01-26 广州视源电子科技股份有限公司 机器人状态检测方法、装置及设备
CN112269187B (zh) * 2020-09-28 2024-05-14 广州视源电子科技股份有限公司 机器人状态检测方法、装置及设备
CN113566737A (zh) * 2021-07-28 2021-10-29 赤湾集装箱码头有限公司 岸边起重机船体离梆监测方法及其岸边起重机

Similar Documents

Publication Publication Date Title
CN110954100A (zh) 一种基于激光与惯导融合的足式机器人本体状态估计方法
CN108090958B (zh) 一种机器人同步定位和地图构建方法和系统
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
JP4148276B2 (ja) 位置推定装置、位置推定方法及びプログラム記録媒体
US7696894B2 (en) Method for determining a relative position of a mobile unit by comparing scans of an environment and mobile unit
CN111402339B (zh) 一种实时定位方法、装置、系统及存储介质
CN112965063B (zh) 一种机器人建图定位方法
Chiu et al. Robust vision-aided navigation using sliding-window factor graphs
CN113624221B (zh) 一种融合视觉与激光的2.5d地图构建方法
KR101214143B1 (ko) 이동체의 위치 및 방향 인식 장치 및 그 방법
CN107179082B (zh) 基于拓扑地图和度量地图融合的自主探索方法和导航方法
CN111076733A (zh) 一种基于视觉与激光slam的机器人室内建图方法及系统
CN108489486A (zh) 二维码以及用于机器人的视觉-惯性组合导航系统及方法
CN109001789B (zh) 一种基于互相关熵配准的无人车定位融合方法
CN109074638B (zh) 融合建图方法、相关装置及计算机可读存储介质
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN112435262A (zh) 基于语义分割网络和多视图几何的动态环境信息检测方法
CN116429084A (zh) 一种动态环境3d同步定位与建图方法
CN111664857A (zh) 自己位置估算装置、自己位置估算方法及记录介质
CN111882602A (zh) 基于orb特征点和gms匹配过滤器的视觉里程计实现方法
CN115880364A (zh) 基于激光点云和视觉slam的机器人位姿估计方法
Moore et al. Simultaneous local and global state estimation for robotic navigation
WO2022110767A1 (zh) 定位与地图构建方法、装置、机器人及计算机存储介质
CN112067007B (zh) 地图生成方法、计算机存储介质及电子设备
CN111121779B (zh) 一种无人机所处飞行区域的实时检测方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication

Application publication date: 20200403

RJ01 Rejection of invention patent application after publication