狭小检测空间下多传感融合四足机器人自主导航策略

摘 要：针对狭小检测空间的机器人巡检任务需求，提出多传感适时融合的小型四足机器人自主导航策略。采用惯性测量单元和激光测距传感器进行卡尔曼适时融合滤波获取机器人位姿；引入模型预测控制实现对期望路径的高精度鲁棒跟踪，完成机器人狭小空间内的自主导航目标。试验验证表明：机器人实际行进路径与期望路径的吻合度较好，基于该策略可以实现小型四足机器人在狭小空间的自主导航目标。

关键词：四足机器人；自主导航；狭小空间；多传感适时融合算法；模型预测控制；局部路径跟踪

中图分类号：TP242 文献标志码：A 文章编号：1671-5276（2024）04-0214-05

Autonomous Navigation Strategy for Multi-sensor Fusion Quadruped Robots in Confined Spaces

XING Yuhang¹，ZHANG Zhourong¹，YUAN Jiwei¹，DUAN Jinjun²， XUE Jingfeng²， DAI Zhendong¹

（1. College of Mechanical and Electrical Engineering， Nanjing University of Aeronautics and Astronautics， Nanjing 210016， China; 2. Chinese Aeronautical Establishment， Beijing 225111， China）

Abstract：For the demand of robot inspection tasks in confined space， this paper proposes an autonomous navigation strategy for small quadruped robots with multi-sensor timely fusion. Inertial measurement units and laser range sensors are used to obtain the robot's position by timely Kalman fusion filtering， and model predictive control is introduced to achieve high-precision robust tracking of the desired path to complete the robot's autonomous navigation goal in a confined space. The experimental verification shows that the actual path of the robot travels well with the desired path， and the autonomous navigation task of the small quadruped robot in the confined space can be achieved based on this strategy.

Keywords：quadruped robots；autonomous navigationl；confined space; multi-sensor fusion algorithm; model predictive control; local path tracking

0 引言

在航空工业中，诸如机翼舱道、飞机进气道等对于飞机的安全可靠运行有着至关重要的作用，因此针对这些部件的定期维护检修需求很大^[1-2]。此类部件人工难以进入内部检修，使用机器人代替人工进行检修能大大提高效率，而小型四足机器人在此类非结构复杂空间中表现出较强的越障能力和一定的负载能力。

为实现四足机器人在狭小空间内的检测任务，需要四足机器人具备在环境中的精确定位并跟踪轨迹的能力^[3]。在机器人导航问题中状态估计和路径跟踪是导航框架中两个重要环节，浙江大学的ZHANG等^[4]通过三维激光雷达融合IMU及运动学里程计实现机器人的状态感知，并通过动力学建模实现对机器人轨迹的滚动时域控制。中国北方车辆研究所的刘宇飞等^[5]采用基于雷达点云信息的全局SLAM定位系统，提出了一种基于拓扑度量相结合的定位框架，实现了在室外环境的地形建模和定位，并利用Dijkstra算法和人工势场理论实现了全局及局部的路径规划和自主避障。ZHANG等^[6]提出了一套轻量化的四足机器人导航系统FSR-SLAM，该系统利用深度相机和二维激光雷达完成机器人状态感知。MIT的KIM等^[7]在Mini-Cheetah上通过RealSense深度相机实现了机器人对地形的高度感知。ETH ANYmal团队的TRANZATTO等^[8]利用互补的多模态传感器融合方法（CompSLAM）完成机器的自主导航，其融合了视觉和热成像、激光雷达深度数据、惯性和运动学位姿估计，以分层方式提供针对传感器数据的融合处理。

不同于一般的四足机器人自主导航问题，狭小空间中的导航问题有一定的特殊性，在诸如空间结构尺寸、表面材质、信号屏蔽、障碍物处理等问题上都有独特的限制条件和边界情况，由此也带来了机器人导航方式和传感器选取上的限制。针对上述问题，本文提出采用高精度激光测距传感器和IMU数据融合实现机器人的状态估计，在此基础上结合机器人的失稳边界，对期望路径进行基于模型预测控制算法的闭环控制，为狭小复杂空间下的四足机器人自主导航提供了一种可行的思路。

1 四足机器人自主导航策略

在狭小空间的检测任务中，为实现机器人在指定位置的准确定点启停，以更好地完成检测任务，本文提出在已知地图的指定位置及机器人状态切换节点设立信标点，从而将全局地图转换为依次连续的以各信标点为起点的局部地图。

根据所在局部地图特性，设计机器人的运行状态以及期望路径，并对机器人在该局部地图内的位姿状态进行估计，为使机器人获得更准确、鲁棒的状态估计，采用激光测距传感器和惯性测量单元进行卡尔曼滤波最优融合，并以融合后的估计值与期望路径作为输入，通过模型预测控制输出机器人的期望速度，以实现机器人在每个局部内的高精度路径跟踪（图1）。

2 多传感适时融合位姿估计

机器人对自身位置的准确估计是实现路径跟踪的前提，机器人在环境中行走时存在以下问题：

1）地面情况不平整，由于自身重力作用、足底与界面间的打滑及传动误差等因素，自身的运动学里程计会产生较大偏移；

2）使用IMU进行积分可以估计机器人自身状态，但无法长时间准确估计机器人在环境中的精确位姿；

3）激光测距传感器会受到环境中干扰因素的影响，使数据产生毛刺。

为解决上述问题，本文提出一种基于卡尔曼滤波的多传感适时融合算法，利用激光测距传感器的高精度特性及IMU基于机器人自身状态不受环境干扰的特性，对激光测距传感器和IMU数据进行融合滤波。

2.1 传感器数据预处理

IMU固连安装于机器人本体构成捷联系统，因此基于IMU的观测都在机器人本体坐标系下，对陀螺仪的观测进行积分可以得到机器人本体姿态；对加速度计的观测进行积分，并通过陀螺仪获取的姿态进行变换转换到大地坐标系下，消除重力加速度影响后，对观测进行二次积分，最终可以得到机器人本体相对于大地坐标系的位置。

激光测距传感器（LRS）可以适用于多种不同工况，不受光照条件、表面材质的影响，可直接获取与环境间的高精度相对位置信息，因此不存在累计误差，但同时在受环境因素干扰时，会产生数据失真现象。

在本方案中，机器人机身周围共设置6个激光测距传感器，分别位于机器人左右两侧及后方。

在图2所示局部地图中，通过位于机器人左侧的两个激光测距传感器进行对机器人侧边距和偏航角的估计，令大地坐标系原点与环境立面的侧边距为e，可以得到机器人本体坐标系对于大地坐标系的状态估计。

2.2 基于卡尔曼滤波的传感器适时融合算法

由于卡尔曼滤波算法仅需储存前一时刻的状态量，因此适用于机器人搭载的嵌入式系统，并具有很好的实时性。

考虑机器人的运动动态系统模型为

式中：X_k+1=[P_yΨ P_y·Ψ·]为系统下一时刻的系统状态变量；X_k为当前时刻的系统状态变量；A为状态转移矩阵；U_k为控制输入；B为控制增益矩阵；W_k为运动模型的噪声；Z_k为传感器的观测量；H为传感器观测矩阵；V_k为传感器观测噪声。该适时融合方法的主要过程（图3）如下。

1）传感器初始化，以机器人局部地图坐标原点作为起点，将机器人静止状态时状态变量作为起始状态。

2）状态预测，对机器人状态进行预测。

3）传感器测量，通过IMU和LRS分别得到传感器的测量值，记作Z_IMU和Z_LRS。为消除累计误差对估计值的影响，IMU仅更新速度的变化量。

4）阈值判断，当LRS检测到突变并判断其大于设定阈值时，仅利用IMU在此时融合结果基础上进行估计，并在恢复后回到融合滤波模式。

5）状态更新，对卡尔曼增益K′进行更新，X︿′_k为对应传感器的本次预测更新的系统状态向量的最优估计值，同时进行对应协方差矩阵P′_k的更新。

图3 卡尔曼适时融合滤波算法流程图

基于该融合滤波方法，既可以利用激光测距传感器实现机器人在局部环境中的高精度、无累计误差的状态估计，又可以利用IMU短时运动估计不受环境变化影响的特点，有效增加了系统应对环境干扰时的鲁棒性和长期状态估计的准确无偏。

3 基于模型预测控制的局部路径跟踪算法

在本自主导航策略中，采用模型预测控制实现机器人对于局部地图中的期望路径进行跟踪。将融合滤波后的状态估计作为输入，通过模型预测控制输出机器人速度，并作为期望值输入机器人底层运动控制环节。

模型预测控制算法由运动学模型建立、模型预测、构建误差目标函数、优化求解这几部分构成。

记控制步长为N，结合前文得到的机器人运动模型便可以得到预测时域内的机器人状态预测模型为X=A_qpx_k+B_qpU。

取ΔT为时间间隔，离散得到一系列机器人位姿的期望值X_k+i，ref（i=1，2，3，…，N）。

构建目标函数时，希望跟踪误差及控制量都尽可能小，因此以此建立目标函数：

构造二次规划问题，以实现问题的最优化。其中第一项与位置的跟踪精度对应，第二项控制机器人速度，这在四足机器人的平衡稳定运动中起着相当的作用。

在此基础上，根据实际情况，同时需要对输入输出做一些限制，由于本文研究的环境为狭小环境，同时要求一定的作业精度，因此将最大允许的侧向工作范围作为其中一个约束，通过c_min和c_max表示，另外由机器人速度失稳边界产生对控制量的约束，通过U_min和U_max表示，将模型预测控制问题描述为：

式中C为参数矩阵。

采用二次规划对此问题进行求解。

4 狭小检测环境下机器人自主导航试验验证

4.1 仿真测试

在Webots软件中搭建仿真环境进行试验，选取其中一个局部地图对基于卡尔曼滤波的多传感适时融合算法进行测试（图4）。

从图4中可以看出，该滤波算法在侧向距离和偏航角两个维度上都可以有效过滤突变数据，增强了机器人在环境存在干扰工况下的鲁棒性。

设定期望偏航角及侧向距离并选取采样区间对机器人位姿进行测量，得出结果如图5所示。

由此可知，在机器人行进时，通过模型预测控制进行对期望轨迹的跟踪有显著效果，对侧向距离的控制可以精确到±0.01m，同时对偏航角的控制可以精确到±0.02rad。

4.2 实物测试与验证

实物试验主要基于上述仿真试验的结果，对实物机器人在机翼舱道内轨迹跟踪进行测试。机器人的各项参数及实物试验中的各项参数选取如表1所示。

根据本文提出的机器人自主导航策略，首先对机器人的路径跟踪能力进行验证，由于机器人在局部地图连接时存在一定的误差，因此分别验证机器人在不同初始位姿偏置情况下的路径跟踪情况（图6）。

在图6中4种不同初始偏置情况下，分别记录机器人侧向位移与偏航角的变化情况（图7）。

从图7数据可以分析得出，机器人样机在不同初始偏置情况下的位姿变化情况，在初始均无偏置情况下，机器人侧向距离稳定在期望距离±15mm以内，偏航角稳定在±0.1rad内。当存在初始侧向偏置时，机器人均可在6s内稳定在期望位置与姿态，由此说明机器人样机可以在不同偏置情况下快速收敛到局部地图的期望路径上，完成机器人在舱道内的高精度路径跟踪。在此基础上，根据上文基于信标点的全局地图离散方法，通过状态机的判断和切换进行局部地图间的连接，以此实现机器人在全局地图上的自主导航（图8）。

5 结语

本文针对狭小检测空间的机器人巡检任务需求，进行了四足机器人自主导航策略的研究，提出了多传感适时融合位姿估计算法以及模型预测控制路径跟踪算法。实验结果表明：采用基于卡尔曼融合滤波的多传感适时融合算法，机器人在舱道内行进时可以较好地过滤角铁带来的传感器失真，在复杂环境中为机器人提供稳定准确的高精度状态估计。在此基础上，基于模型预测控制，在机器人失稳边界内得出多目标下的最优期望速度，实现机器人反馈控制，精确稳定地对机器人期望路径进行实时跟踪，通过任务状态机控制联接各局部，实现机器人在狭小复杂空间下四足机器人的自主导航。

参考文献：

[1] NEGRI S P， BASILE V， VALORI M， et al. A modular mobile robotic architecture for defects detection and repair in narrow tunnels of CFRP aeronautic components[J]. Robotics and Computer-Integrated Manufacturing， 2019， 55： 109-128.

[2] JENA S，GUPTA A. Embedded sensors for health monitoring of an aircraft[M]//Energy，Environment，and Sustainability. Singapore：Springer Singapore，2018：77-91.

[3] FERREIRA J，MOREIRA A P，SILVA M，et al. A survey on localization，mapping，and trajectory planning for quadruped robots in vineyards[C]//2022 IEEE International Conference on Autonomous Robot Systems and Competitions （ICARSC）. Santa Maria da Feira，Portugal：IEEE，2022：237-242.

[4] ZHANG Z，YAN J Q，KONG X，et al. Efficient motion planning based on kinodynamic model for quadruped robots following persons in confined spaces[J]. IEEE/ASME Transactions on Mechatronics，2021，26（4）：1997-2006.

[5] 刘宇飞，江磊，邢伯阳，等. 基于全局SLAM的四足机器人路径规划研究[J]. 无人系统技术，2021，4（6）： 57-64.

[6] ZHANG C，YANG Z，FANG Q H，et al. FRL-SLAM：a fast，robust and lightweight SLAM system for quadruped robot navigation[C]//2021 IEEE International Conference on Robotics and Biomimetics （ROBIO）. Sanya，China：IEEE，2022：1165-1170.

[7] KIM D，CARBALLO D，DI CARLO J，et al. Vision aided dynamic exploration of unstructured terrain with a small-scale quadruped robot[C]//2020 IEEE International Conference on Robotics and Automation （ICRA）. Paris，France：IEEE，2020：2464-2470.

[8] TRANZATTO M，MIKI T，DHARMADHIKARI M，et al. CERBERUS in the DARPA subterranean challenge[J]. Science Robotics，2022，7（66）：eabp9742.

收稿日期：2023-02-15