[发明专利]基于路标信息与多传感器数据融合的机器人重定位方法

权利要求书

1.基于路标信息与多传感器数据融合的机器人重定位方法，其特征在于，包括以下步骤：

步骤S1：构建地图，并在构建已知地图的过程中同时设置并确定几个特征明显的物体作为系统中的路标，在路标物体的选择和确定路标物体在当前环境的位置要符合本方法的基本原则；

步骤S2：利用聚类算法将落在同一路标物体上的激光点聚成一个激光点簇，形成激光数据；

步骤S3：通过深度学习和图像匹配的方式对视觉识别的物体产生视觉语义信息；

步骤S4：利用标定参数和几何模型，将激光数据和视觉语义信息进行信息融合，产生语义激光数据，给环境中每个路标物体对应的激光簇数据赋予视觉语义标签；

步骤S5：基于语义激光数据，构建带有各路标名称的语义地图，并且根据一一对应的路标名称和位置坐标建立关联映射表；

步骤S6：当定位出现偏差时，导航过程中利用视觉识别附近路标物体的分类及语义名称；

步骤S7：根据路标的语义信息查找映射表中对应坐标；

步骤S8：根据路标的坐标信息、激光实时感知数据和IMU记录角度信息利用转换公式反向推算机器人当前实际位置。

2.根据权利要求1所述的基于路标信息与多传感器数据融合的机器人重定位方法，其特征在于，所述的步骤S1的具体步骤如下：

步骤S101：地标物体的选择和设置

建立一个自带地标物体的环境地图，完成对地标物体的设定，而其设定要符合如下两个基本原则：

(1)挑选特征较为明显或者自带辨识信息的物体作为路标；

(2)路标为固定的静态物体，如果是位置或者形状在动态变化的物体则拒绝选择；

步骤S102：确定地标物体在当前环境的位置；

选定好地标物体之后，将路标放置在当前环境中，放置地点满足以下两个约束条件：

(1)由于路标的状态是长期静止不动的，为了不影响机器人和其他事物的运动，所以路标的放置位置应在靠墙或者角落；

(2)各个路标的摆放位置要覆盖到整个地图环境；

步骤S103：构建地图

在选择好的地标物体按照放置原则确定安放在地图环境当中之后，便可构建地图，获取精准的地图信息。

3.根据权利要求2所述的基于路标信息与多传感器数据融合的机器人重定位方法，其特征在于，所述的步骤S6中所述的视觉识别路标物体的方法采用Tiny-Yolo方法。

4.根据权利要求3所述的基于路标信息与多传感器数据融合的机器人重定位方法，其特征在于，所述的步骤S8的具体步骤如下：

假设机器人所用传感器为激光雷达，雷达建立的坐标系以极坐标表示，雷达扫描激光数据点间隔是0.5度，扫描范围是正前方的180度，则可以扫描360个数据点；而雷达的测量数据点是按角度顺序排列的，则可知第i个数据点在雷达为中心的极坐标系中的角度θ_i为：

其中，θ_i表示第i个数据点所在角度，π表示180°的弧度值，可通过公式(2)(3)将极坐标转化成笛卡尔坐标：

x′_i＝ρ_i·cosθ_i (2)

其中，x′_i表示第i个数据点横坐标，ρ_i表示机器人到第i个数据点的距离，cosθ_i表示第i个数据点的余弦值；

y′_i＝ρ_i·sinθ_i (3)

其中，y′_i表示第i个数据点纵坐标，ρ_i表示机器人到第i个数据点的距离，sinθ_i表示第i个数据点的正弦值；

将第i个数据点在笛卡尔坐标系中的点M点看作是算法当中的地标物体，通过映射表已知其世界坐标通过公式(1)(2)(3)求得其局部坐标(x′_i，y′_i)，并且根据IMU的测量值可以得到机器人的航向角，即局部坐标系相对世界坐标系的偏转角α，来推算机器人的当前实际位姿：

设O为世界坐标系的原点；O’为传感器数据源，即机器人的实时位置，以其为原点构建了一个局部坐标系，机器人当前相对世界坐标系的位置坐标(x₀，y₀)可表示为：

其中，x₀表示机器人位置横坐标，表示地标物体的横坐标，l_x1表示数据点投影的总长，l_x2表示数据点投影的部分长度；

其中，y₀表示机器人位置纵坐标，表示地标物体的纵坐标，l_y1表示数据点纵轴投影的部分长度，l_y2表示数据点纵轴投影的部分长度；L的长度可以根据地标M的局部坐标长度和机器人航向角α求得：

l_x1＝x′_i·cosα (6)

其中，x′_i表示第i个数据点横坐标，cosα表示机器人偏转角的余弦，l_x1表示数据点投影的总长；

l_x2＝y′_i·sinα (7)

其中，y′_i表示第i个数据点纵坐标，sinα表示机器人偏转角的正弦，l_x2表示数据点投影的部分长度；

l_y1＝y′_i·cosα （8)

其中，y′_i表示第i个数据点纵坐标，cosα表示机器人偏转角的余弦，l_y1表示数据点纵轴投影的部分长度；

l_y2＝x′_i·sinα (9)

其中，x_i′表示第i个数据点横坐标，sinα表示机器人偏转角的正弦，l_y2表示数据点纵轴投影的部分长度；

综合公式(4)(5)(6)(7)(8)(9)可推出机器人相对世界坐标系的坐标为：

其中，x₀表示机器人位置横坐标，表示地标物体的横坐标，x_i′表示第i个数据点横坐标，cosα表示机器人偏转角的余弦，y′_i表示第i个数据点纵坐标，sinα表示机器人偏转角的正弦；

其中，y₀表示机器人位置纵坐标，表示地标物体的纵坐标，x′_i表示第i个数据点横坐标，cosα表示机器人偏转角的余弦，y′_i表示第i个数据点纵坐标，sinα表示机器人偏转角的正弦；

即当机器人由于航程推演方法缺陷、地面环境不均匀、人为移动机器人位置而产生位姿偏差时，可以通过以上算法推算机器人实际位置。