一种基于激光与惯导融合的足式机器人本体状态估计方法与流程
本发明涉及一种足式机器人本地运动状态的技术领域，尤其涉及一种基于激光与惯导融合的足式机器人本体状态估计方法。
背景技术：
移动机器人需要对其状态进行良好的估计，才能在结构化和非结构化环境中进行闭环控制。许多现有的算法都依赖来自多个传感器的数据融合以计算这些估计值。该技术领域常用本体感应传感器的测量信息，如加速度、机器人内部的电机速度、关节角度，以及来自外部特征的测量信息，如光强度、距离测量、声音振幅等进行融合，然而这些数据来源并不总是可靠的。轮式机器人通常使用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集映射到参考扫描点集所在的坐标系中，并记为
其中，pj为待匹配扫描点，
对于所述每个点
基于所述距离最近的两个点的索引
可选的，所述基于所述帧间匹配的结果，通过计算得到增量运动变化包括：
基于所述帧间匹配的结果，对经典icp的误差函数进行修改，通过计算得到点到线段距离的平方和，具体计算公式如下：
其中，cn为第n步中所有点到线段的对应关系，qn+1为下一次的位姿变换估计值，r(θn+1)和tn+1分别表示下一次的两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵，
通过求解所述误差函数的最小值，得到下一次的位姿变换估计值，具体公式如下：
其中，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映射到参考点集坐标系下的点记为
其中，pj为待匹配扫描点，
对于所述每个点
基于所述距离最近的两个点的索引
基于所述帧间匹配的结果，对经典icp的误差函数进行修改，通过计算得到点到线段距离的平方和，具体计算公式如下：
其中，cn为第n步中所有点到线段的对应关系，qn+1为下一次的位姿变换估计值，r(θn+1)和tn+1分别表示下一次的两组点集位姿变化估计的旋转变换矩阵和平移变换矩阵，
通过求解所述误差函数的最小值，得到下一次的位姿变换估计值，具体公式如下：
其中，sref为通过参考扫描点集获取的一个参考表面集；
基于所述位姿变换估计值，得到足式机器人位姿变化，通过进一步计算可得到足式机器人本体的速度，进而得到增量运动变化。需要说明的是，重复以上步骤进行n次迭代，直到算法收敛或循环结束，完成两幅扫描数据的匹配过程，结合附图3所示，图3示出本发明实施例中的plicp算法的迭代示意图。
另外，具体的，所述基于点线icp中的plicp算法，对所述激光扫描数据进行帧间匹配还包括快速搜索对应点对的策略，并且适用于传统icp算法的对应点对搜索。原始的搜索策略是遍历序列中所有样本点，优化算法改进了搜索范围，在每个参考帧坐标系中的目标搜索点的搜索区间，结合附图4所示，附图4示出本发明实施例中的混合快速搜索关联点策略示意图a，往极坐标的中心视角方向分别左右增加一定角度范围
可见，原始的搜索策略搜索空间冗余，且不一定存在转换关系的先验知识，基于此，混合以下几种策略共同作用进行改善搜索速度，具体如下：
(1)搜索起始点选择；结合附图5所示，附图5示出本发明实施例中的混合快速搜索关联点策略示意图b；起始搜索索引由当前扫描帧目标点pj的前一点pj-1的最近邻点
(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，readonlymemory)、随机存取存储器(ram，randomaccessmemory)、磁盘或光盘等。
另外，以上对本发明实施例所提供的一种基于激光与惯导融合的足式机器人本体状态估计方法进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。
- 还没有人留言评论。精彩留言会获得点赞！