一种基于地图雷达点云的巡检机器人定位方法与流程

1.本发明涉及一种基于地图雷达点云的巡检机器人定位方法。

背景技术：

2.巡检型轮式移动机器人是一种使用四轮在地面移动的机器人，普遍需要具备自主导航能力，其中一项关键的技术就是对机器人的实时定位。市面上比较通用的方式是采用传感器融合的方式(具体地：将雷达、imu、轮速计、相机等数据进行融合来达到定位。)，传感器融合方式的实质是构建一个非线性最小二乘问题，使用迭代法将各部分观测的产生的残差降低。本发明主要在于原先多传感器融合定位技术的基础上进一步降低残差。

技术实现要素：

3.为达到，本发明提供一种基于地图雷达点云的巡检机器人定位方法，包括以下步骤：
4.s1:根据实时估算车轮半径，以及车轮中心点姿态，车轮与地面接触点的姿
5.态t_wbi_tom＝t_wi_tom*[0,0,-r_i,1]；
[0006]
s2:将四个车轮与地面的接触点进行平面拟合，得到估算出来的车轮与地面的接触面n；
[0007]
s3:以4个车轮与地面接触点作为索引，在场景点云地图p中分别到最近的a个点，然后分别进行平面拟合，得到属于地图点云的4个平面 m1,m2,m3,m4
[0008]
s4:构建n与mi的平面夹角a_i
[0009]
s5:根据平面夹角构建新的残差f_k＝[a_1,a_2,a_3,a_4]
[0010]
s6:采用非线性最小二乘方法来得到当前的车身姿态
[0011][0012]
进一步地，所述a个点为6个点。
[0013]
本发明的有益效果是：本发明中方法是在原先多传感器融合定位技术的基础上，再添加一种机器人位于地面的约束，来帮助提高机器人的定位稳定性以及精度。
具体实施方式
[0014]
以下结合实施例对本发明作进一步的阐述，所述的实施例仅为本发明一部分的实施例，这些实施例仅用于解释本发明，对本发明的范围并不构成任何限制。
[0015]
本申请实施例中所示的流程仅是示例说明，不是必须包括所有的内容和操作/步骤，也不是必须按所描述的顺序执行。例如，有的操作/步骤还可以分解、组合或部分合并，因此实际执行的顺序有可能根据实际情况改变。
[0016]
为了了解本发明的方案，首先对以下参数进行设定：场景的点云地图：p, 四个车轮的中心点姿态：t_wi_tom(i＝1,2,3,4),四个车轮的实时估算半径：r_i (i＝1,2,3,4),
传感器融合中当前姿态估计的残差r具体包括雷达匹配残差：fi、视觉残差：fj；
[0017][0018]
本发明提供一种基于地图雷达点云的巡检机器人定位方法，包括以下步骤；
[0019]
s1:根据实时估算车轮半径r_i，以及车轮中心点的坐标系t_wi，计算出车轮与地面接触点的坐标系t_wbi
[0020][0021]
s2:进行预测平面拟合得到预测值:具体地，将四个车轮与地面的接触点进行平面拟合，得到估算出来的车轮与地面的接触面n；其中，
[0022]
s3:进行观测平面拟合得到观测值：具体地，以4个车轮与地面接触点作为索引，在场景点云地图p中分别到最近的a个点(优选地为6个点)，然后分别进行平面拟合，得到属于地图点云的4个平面m1、m2、m3、m4；
[0023]
s4:构建预测值与观测值之间的残差：f_k＝a_i＝[a_1,a_2,a_3,a_4], 其中，a_i代表n与mi的平面夹角；
[0024]
s6:采用非线性最小二乘方法来得到当前的巡检机器人的位置；
[0025][0026]
本发明中方法是在原先多传感器融合定位技术的基础上，再添加一种机器人位于地面的约束，来帮助提高机器人的定位稳定性以及精度。
[0027]
以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：

1.一种基于地图雷达点云的巡检机器人定位方法，其特征在于：包括以下步骤；s1:根据实时估算车轮半径r_i，以及车轮中心点的坐标系t_wi，计算出车轮与地面接触点的坐标系t_wbis2:进行预测平面拟合得到预测值:具体地，将四个车轮与地面的接触点进行平面拟合，得到估算出来的车轮与地面的接触面n；其中，s3:进行观测平面拟合得到观测值：具体地，以4个车轮与地面接触点作为索引，在场景点云地图p中分别到最近的a个点，然后分别进行平面拟合，得到属于地图点云的4个平面m1、m2、m3、m4；s4:构建预测值与观测值之间的残差：f_k＝a_i＝[a_1,a_2,a_3,a_4],其中，a_i代表n与mi的平面夹角；s6:采用非线性最小二乘方法来得到当前的巡检机器人的位置；2.根据权利要求1所述的一种基于地图雷达点云的巡检机器人定位方法，其特征在于：其中，所述a个点为6个点。

技术总结

本发明涉及一种基于地图雷达点云的巡检机器人定位方法，包括以下步骤；S1:根据实时估算车轮半径r_i，以及车轮中心点的坐标系T_wi，计算出车轮与地面接触点的坐标系T_wbiS2:进行预测平面拟合得到预测值:具体地，将四个车轮与地面的接触点进行平面拟合，得到估算出来的车轮与地面的接触面N；S3:进行观测平面拟合得到观测值：具体地，以4个车轮与地面接触点作为索引，在场景点云地图P中分别到最近的A个点，然后分别进行平面拟合，得到属于地图点云的4个平面M1、M2、M3、M4；S4:构建预测值与观测值之间的残差：f_k＝a_i＝[a_1,a_2,a_3,a_4],其中，a_i代表N与Mi的平面夹角；S6:采用非线性最小二乘方法来得到当前的巡检机器人的位置；来得到当前的巡检机器人的位置；来得到当前的巡检机器人的位置；