CN111982114B - 一种采用imu数据融合估计三维位姿的救援机器人 - Google Patents

Abstract

本发明公开了一种采用IMU数据融合估计三维位姿的救援机器人，所述机器人包括移动平台，在移动平台上搭载有图像采集模块、距离信息采集模块、数据处理模块、运动控制模块和远程控制模块，其中：所述图像采集模块用于采集机器人所处环境中的图像信息，以搜寻待救援者；所述距离信息采集模块用于通过惯性测量单元获取IMU数据，并通过激光雷达获取激光点云数据；所述远程控制模块用于接收工控机端发送的控制指令；所述运动控制模块用于生成当前的环境地图，并将待救援者的位置在环境地图上标注出来；根据所述的控制指令，使机器人通过路径规划和避障算法到达目标点；所述数据处理模块用于对IMU数据和激光点云数据进行融合，以估计机器人的三维位姿。

Description

一种采用IMU数据融合估计三维位姿的救援机器人

技术领域

本发明涉及机器人及建图领域，具体涉及一种采用惯性测量(Inertialmeasurement unit,IMU)数据融合估计三维位姿的救援机器人。

背景技术

自主式移动机器人对于自身位姿的估计和对周围环境的感知和建模，即建图(Simultaneous Localization And Mapping)，是移动机器人真正实现自主地在真实世界的未知环境中运行的最重要的能力之一。由于激光雷达具有十分精确的测距性能，以及较强的抗干扰能力，所以，基于激光雷达的建图方法已经引起了工程师们极大的关注。许多基于占据栅格地图的导航系统，例如开源算法GMapping，Hector建图和Cartographer已经应用于室内机器人导航，并在室内环境下，表现良好。

基于激光雷达的建图算法可以精准地构建出简单环境下的2D地图，但是激光雷达并不能采集到机器人的三维高度信息，所以机器人在复杂的救援环境中难以进行三维位姿的估计，并构建准确的地图，因此难以进行自主探索。例如，激光雷达会把救援环境中的斜坡识别为障碍物。

基于三维点云信息的建图算法虽然可以提供机器人三维位姿估计和环境地图，但是需要多线激光雷达传感器提供信息。多线激光雷达成本较高并且体积较大，不利于小型救援机器人的低成本和小型化，并且这类算法计算量较大而且对参数的设置非常敏感。基于视觉的建图算法虽然成本低廉，但是这类算法的鲁棒性有待提升，而且同样存在计算量大的问题。类似于救援环境这样不平坦的环境，激光雷达不能采集到机器人的三维高度信息，导致机器人在复杂的环境中难以自主探索。

发明内容

本发明的目的是提供一种采用IMU数据融合估计三维位姿的救援机器人，通过对IMU数据以及点云数据的融合，改善了传统建图建图上的缺陷。

为了实现上述任务，本发明采用以下技术方案：

一种采用IMU数据融合估计三维位姿的救援机器人，所述机器人包括移动平台，在移动平台上搭载有图像采集模块、距离信息采集模块、数据处理模块、运动控制模块和远程控制模块，其中：

所述图像采集模块用于采集机器人所处环境中的图像信息，以搜寻待救援者；

所述距离信息采集模块用于通过惯性测量单元获取IMU数据，并通过激光雷达获取激光点云数据；

所述远程控制模块用于接收工控机端发送的控制指令；

所述运动控制模块用于生成当前的环境地图，并将待救援者的位置在环境地图上标注出来；根据所述的控制指令，使机器人通过路径规划和避障算法到达目标点；其中，所述当前的环境地图的具体生成过程为：

对前后采集到的激光点云数据进行扫描匹配，通过局部地图对机器人的位姿进行估计，将局部环境地图融合为全局环境地图；当激光点云数据和现有环境地图对齐时，与所有先前的激光点云数据隐式地执行匹配；

对于激光点云数据和已经生成环境地图之间的最优匹配，首先要估计出机器人的位姿ξ＝(x,y,ψ)，其中x、y是机器人在水平面上的坐标，ψ是偏航角，使得下式取最小值：

其中，S_i(ξ)是位姿ξ的函数，表示激光点云数据在环境地图上的坐标S_i＝(s_i,x,s_i,y)，n表示激光点云数据的数量；M(S_i(ξ))是S_i(ξ)的函数，表示S_i(ξ)坐标处地图的占用值；然后得到激光点S_i(ξ)的估计值：

上式中，(p_x,p_y)表示前一时刻激光点云数据在环境地图上的坐标；

给定位姿ξ的一些初始估计，并且已知：

其中Δξ是ξ的误差量，求M(S_i(ξ+Δξ))的一阶泰勒展开式可使得Δξ误差趋于最小值：

其中，

为矢量微分算符；上式在相对于Δξ的偏导数为0时取最小值，即：

通过上式求得Δξ：

其中，

由此得到Δξ的最小值，即得到机器人位姿ξ的最佳估计值；

所述数据处理模块用于对IMU数据和激光点云数据进行融合，以估计机器人的三维位姿，包括：

通过IMU数据得到机器人当前的翻滚角

和俯仰角θ，然后通过航迹推算估计机器人高度z，通过方差加权平均算法融合航迹推算，以更新机器人当前高度z；基于这一系列数据的融合和计算，最终可以实时估计机器人在三维空间中的位姿(x,y,z,ψ,

θ)。

进一步地，所述通过航迹推算法估计机器人高度z_t，通过方差加权平均算法融合航迹推算法，以更新机器人当前高度，包括：

机器人在水平方向的位移δ可以根据环境地图中机器人水平方向x,y坐标的变化Δx,Δy求出：

机器人前进方向为δ正方向，机器人当前高度估计

由上一时刻的高度值

俯仰角θ以及位移δ计算得到：

将机器人所在环境地图中栅格的高度加上机器人底盘到机器人中心的高度的期望值h_t、方差值

表示为

航迹推算法得到的高度预测值

及方差值

表示为

则机器人此时在三维空间的高度坐标z_t以及其方差

可以通过方差加权平均算法融合这两个高度值得到：

进一步地，所述根据所述的控制指令，使机器人通过路径规划和避障算法到达目标点，包括：

在当前生成的环境地图的基础上，生成代价地图，然后使用基于边界的探索方式对未知环境进行探索，分阶段地生成目标点，根据这些目标点的位置使用A*全局路径规划算法计算出一条最短且可行的路径；在救援机器人达到全局路径所生成的目标点之前，使用DWA局部路径规划算法对机器人附近的障碍物进行动态避障，最后，如果机器人在前往局部路径点的过程中，出现异常情况，则进行全局和局部的路径规划。

进一步地，通过路由器搭建WIFI网络，机器人的远程控制模块通过WIFI网络实现与工控机端的通信，工控机端通过WIFI网络获取机器人的数据信息并对机器人的运行状态进行实时监控。

进一步地，所述机器人的移动平台采用履带式底盘。

进一步地，通过设置救援机器人的路径规划算法，然后再打开导航节点，让机器人在救援环境中自主行驶，同时探索未知环境，找到受困者的具体位置。

与现有技术相比，本发明具有以下技术特点：

本发明采用IMU数据融合以得到机器人的三维位姿信息。首先基于单线激光雷达进行2D建图，估计机器人的二维位姿，包括在地面上的投影位置和航向角，同时创建二维栅格地图；然后从惯性测量单元获得姿态信息，使用其中无漂移的滚转角和俯仰角作为机器人姿态角的估计值，从而得到机器人三维位姿的全部估计。相对于现有救援机器人，本发明的机器人制作成本低，技术实现难度不高；优化了建图算法，在复杂的救援环境下，极大提高了定位精度和建图效果。

附图说明

图1为本发明救援机器人的结构示意图；

图2为本发明一个实施例中设计的救援机器人的实物图；

图3为机器人在实验室中进行测试的建图结果；

图4为机器人在模拟救援场地中的建图结果；

图5为传统建图方法未融合IMU数据的建图效果；

图6为本发明融合IMU数据后的建图效果；

图7为仿真场景的示意图；

图8为机器人自主行驶搜寻的流程示意图。

具体实施方式

参见图1，本发明公开了一种采用IMU数据融合估计三维位姿的救援机器人，所述机器人包括移动平台，在移动平台上搭载有图像采集模块、距离信息采集模块、数据处理模块、运动控制模块和远程控制模块，其中：

本方案中以履带式底盘作为移动平台，其优点是：可以在任意位置进行原地转弯，而且在倾斜的路面上可以保持较好的稳定性，越长性能优越，而且控制难度不高；本申请实施例中机器人的实物图如图2所示。

所述图像采集模块用于采集机器人所处环境中的图像信息，以搜寻待救援者；参见图2，本实施例中图像采集模块包括设置在救援机器人前端、后端的前摄像头、后摄像头。

所述距离信息采集模块用于通过惯性测量单元获取IMU数据，并通过激光雷达获取激光点云数据。其中IMU数据用于测量机器人位姿，提供了更加精确的机器人位置信息，为机器人到岗提供更加精确的位姿信息。

所述远程控制模块用于接收工控机端发送的控制指令，并将机器人的运行数据、位置数据、采集的图像数据发送给工控机端。具体地，通过路由器搭建WIFI网络，机器人的远程控制模块通过WIFI网络实现与工控机端的通信，工控机端通过WIFI网络获取机器人的数据信息并对机器人的运行状态进行实时监控。

所述运动控制模块用于生成当前的环境地图，并将待救援者的位置在环境地图上标注出来；根据所述的控制指令，使机器人通过路径规划和避障算法到达目标点；在此过程中，进行地图的实时构建并通过图像采集模块搜寻待救援者。运动模块为基于Inteli5中央处理器的微型计算机，微型计算机通过USB接口接收激光雷达所回传的信息。

其中，所述当前的环境地图的具体生成过程为：

对前后采集到的激光点云数据进行扫描匹配，通过局部地图对机器人的位姿进行估计，将局部环境地图融合为全局环境地图；当激光点云数据和现有环境地图对齐时，与所有先前的激光点云数据隐式地执行匹配；

对于激光点云数据和已经生成环境地图之间的最优匹配，首先要估计出机器人的位姿ξ＝(x,y,ψ)，其中x、y是机器人在水平面上的坐标，ψ是偏航角，使得下式取最小值：

也就是说，要找到可以使激光扫描与地图最佳对准的变换。其中，S_i(ξ)是位姿ξ的函数，表示激光点云数据在环境地图上的坐标S_i＝(s_i,x,s_i,y)，n表示激光点云数据的数量；M(S_i(ξ))是S_i(ξ)的函数，表示S_i(ξ)坐标处地图的占用值；然后得到激光点S_i(ξ)的估计值：

上式中，(p_x,p_y)表示前一时刻激光点云数据在环境地图上的坐标；

给定位姿ξ的一些初始估计，并且已知：

其中Δξ是ξ的误差量，求M(S_i(ξ+Δξ))的一阶泰勒展开式可使得Δξ误差趋于最小值：

其中，

为矢量微分算符；上式在相对于Δξ的偏导数为0时取最小值，即：

通过上式求得Δξ：

其中，

由此得到Δξ的最小值，即得到机器人位姿ξ的最佳估计值。

本发明的一个实施例中，机器人在实验室中进行测试的建图结果如图3所示，在模拟救援场地中的建图结果如图4所示。

所述数据处理模块用于对IMU数据和激光点云数据进行融合，以估计机器人的三维位姿。为了使得救援机器人在不平坦的救援环境下建立更准确的环境地图，需要解决机器人中的状态估计问题。现有的建图算法并不能完全解决机器人所有的六个自由度的状态，缺少机器人的三维位姿信息，而本发明提出的方法很好的解决了这个问题，将激光雷达的测量数据和IMU数据相结合来估计机器人的位姿，而不是仅仅依靠对IMU的测量值进行积分来对系统状态进行估计。本发明提供的融合方法包括：

航迹推算是从初始已知的坐标位置开始，依据载体在该位置的航向、航速和航行时间，推算下一时刻坐标位置的导航算法。参考该算法的思想，本发明根据机器人在水平方向的位移和机器人俯仰角推算机器人的高度变化。

通过IMU数据得到机器人当前的翻滚角

和俯仰角θ，然后通过航迹推算估计机器人高度z，通过方差加权平均算法融合航迹推算，以更新机器人当前高度z；基于这一系列数据的融合和计算，最终可以实时估计机器人在三维空间中的位姿(x,y,z,ψ,

θ)。

所述通过航迹推算法估计机器人高度z_t，通过方差加权平均算法融合航迹推算法，以更新机器人当前高度，包括：

机器人在水平方向的位移δ可以根据环境地图中机器人水平方向x,y坐标的变化Δx,Δy求出：

机器人前进方向为δ正方向，机器人当前高度估计

由上一时刻的高度值

俯仰角θ以及位移δ计算得到：

将机器人所在环境地图中栅格的高度加上机器人底盘到机器人中心的高度的期望值h_t、方差值

表示为

航迹推算法得到的高度预测值

及方差值

表示为

则机器人此时在三维空间的高度坐标z_t以及其方差

可以通过方差加权平均算法融合这两个高度值得到：

采用传统的建图算法构建的地图效果如图5所示，利用本发明方法将激光点云数据与IMU数据融合之后，利用三维位姿构建的地图效果如图6所示，图7为仿真场景图。可见，IMU数据的融合提高了救援机器人定位的精度，改善了传统建图算法在复杂的救援环境下的性能表现。

在本方案中，所述根据所述的控制指令，使机器人通过路径规划和避障算法到达目标点，包括：

在当前生成的环境地图的基础上，生成代价地图，然后使用基于边界的探索方式对未知环境进行探索，分阶段地生成目标点，根据这些目标点的位置使用A*全局路径规划算法计算出一条最短且可行的路径；在救援机器人达到全局路径所生成的目标点之前，使用DWA局部路径规划算法对机器人附近的障碍物进行动态避障，最后，如果机器人在前往局部路径点的过程中，出现异常情况，则进行全局和局部的路径规划。

通过设置救援机器人的路径规划算法，然后再打开导航节点，让机器人在救援环境中自主行驶，同时探索未知环境，找到受困者的具体位置，机器人的搜寻过程如图8所示。

以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (5)

1.一种采用IMU数据融合估计三维位姿的救援机器人，其特征在于，所述机器人包括移动平台，在移动平台上搭载有图像采集模块、距离信息采集模块、数据处理模块、运动控制模块和远程控制模块，其中：

所述图像采集模块用于采集机器人所处环境中的图像信息，以搜寻待救援者；

所述距离信息采集模块用于通过惯性测量单元获取IMU数据，并通过激光雷达获取激光点云数据；

所述远程控制模块用于接收工控机端发送的控制指令；

所述运动控制模块用于生成当前的环境地图，并将待救援者的位置在环境地图上标注出来；根据所述的控制指令，使机器人通过路径规划和避障算法到达目标点；其中，所述当前的环境地图的具体生成过程为：

对前后采集到的激光点云数据进行扫描匹配，通过局部地图对机器人的位姿进行估计，将局部环境地图融合为全局环境地图；当激光点云数据和现有环境地图对齐时，与所有先前的激光点云数据隐式地执行匹配；

对于激光点云数据和已经生成环境地图之间的最优匹配，首先要估计出机器人的位姿ξ＝(x,y,ψ)，其中x、y是机器人在水平面上的坐标，ψ是偏航角，使得下式取最小值：

其中，S_i(ξ)是位姿ξ的函数，表示激光点云数据在环境地图上的坐标S_i＝(s_i,x,s_i,y)，n表示激光点云数据的数量；M(S_i(ξ))是S_i(ξ)的函数，表示S_i(ξ)坐标处地图的占用值；然后得到激光点S_i(ξ)的估计值：

上式中，(p_x,p_y)表示前一时刻激光点云数据在环境地图上的坐标；

给定位姿ξ的一些初始估计，并且已知：

其中Δξ是ξ的误差量，求M(S_i(ξ+Δξ))的一阶泰勒展开式可使得Δξ误差趋于最小值：

其中，

为矢量微分算符；上式在相对于Δξ的偏导数为0时取最小值，即：

通过上式求得Δξ：

其中，

由此得到Δξ的最小值，即得到机器人位姿ξ的最佳估计值；

所述数据处理模块用于对IMU数据和激光点云数据进行融合，以估计机器人的三维位姿，包括：

通过IMU数据得到机器人当前的翻滚角

和俯仰角θ，然后通过航迹推算估计机器人高度z_t，通过方差加权平均算法融合航迹推算，以更新机器人当前高度；基于这一系列数据的融合和计算，最终可以实时估计机器人在三维空间中的位姿

所述通过航迹推算法估计机器人高度z_t，通过方差加权平均算法融合航迹推算法，以更新机器人当前高度，包括：

机器人在水平方向的位移δ可以根据环境地图中机器人水平方向x,y坐标的变化Δx,Δy求出：

机器人前进方向为δ正方向，机器人当前高度估计

由上一时刻的高度值

俯仰角θ以及位移δ计算得到：

将机器人所在环境地图中栅格的高度加上机器人底盘到机器人中心的高度的期望值h_t、方差值

表示为

航迹推算法得到的高度估计

及方差值

表示为

则机器人此时在三维空间的高度坐标z_t以及其方差

可以通过方差加权平均算法融合这两个高度值得到：

2.根据权利要求1所述的采用IMU数据融合估计三维位姿的救援机器人，其特征在于，所述根据所述的控制指令，使机器人通过路径规划和避障算法到达目标点，包括：

在当前生成的环境地图的基础上，生成代价地图，然后使用基于边界的探索方式对未知环境进行探索，分阶段地生成目标点，根据这些目标点的位置使用A*全局路径规划算法计算出一条最短且可行的路径；在救援机器人达到全局路径所生成的目标点之前，使用DWA局部路径规划算法对机器人附近的障碍物进行动态避障，最后，如果机器人在前往局部路径点的过程中，出现异常情况，则进行全局和局部的路径规划。

3.根据权利要求1所述的采用IMU数据融合估计三维位姿的救援机器人，其特征在于，通过路由器搭建WIFI网络，机器人的远程控制模块通过WIFI网络实现与工控机端的通信，工控机端通过WIFI网络获取机器人的数据信息并对机器人的运行状态进行实时监控。

4.根据权利要求1所述的采用IMU数据融合估计三维位姿的救援机器人，其特征在于，所述机器人的移动平台采用履带式底盘。

5.根据权利要求1所述的采用IMU数据融合估计三维位姿的救援机器人，其特征在于，通过设置救援机器人的路径规划算法，然后再打开导航节点，让机器人在救援环境中自主行驶，同时探索未知环境，找到受困者的具体位置。

Images