一种基于多传感器融合的机器人重定位方法与流程

1.本发明公开一种方法，涉及定位技术领域，具体地说是一种基于多传感器融合的机器人重定位方法。


背景技术：

2.机器人的定位功能主要为机器人提供位置信息，解决“我在哪儿”的问题，是衔接地图与机器人自主导航的关键环节，准确且稳定的定位效果是机器人自主导航的前提。现阶段，常见的机器人定位策略大多基于点云配准的方法，使用当前点云帧与地图点云配准，从而获取较为精确的位置信息。但由于环境特征的影响，常常会出现定位丢失的情况，若没有人为干预，则定位就会失败。


技术实现要素：

3.本发明针对现有技术的问题，提供一种基于多传感器融合的机器人重定位方法，用来对机器人进行自主重定位。
4.本发明提出的具体方案是：
5.一种基于多传感器融合的机器人重定位方法，通过激光雷达加载点云地图，标记地图坐标原点，
6.通过单天线gps以地图坐标原点处经纬度坐标为原点，遵循wgs84坐标系统准则，建立局部平面直角坐标系，
7.获取点云地图原点相对于局部平面直角坐标系的位姿，其中单天线gps提供三轴位置信息，九轴imu提供方向信息，
8.通过点云地图进行点云配准，对于点云配准定位失败的情况，通过单天线gps实时导入gps经纬度信息，将经纬度信息映射至局部平面直角坐标系获得相应点并将所述相应点投影至点云地图获取实时姿态信息，通过九轴imu获取实时方向角信息，利用实时姿态信息和实时方向角信息构成实时重定位位姿，利用实时重定位位姿pc纠正实时点云位姿，重新定位。
9.进一步，所述的一种基于多传感器融合的机器人重定位方法中所述将所述的相应点投影至点云地图，包括：
10.通过点云地图与局部平面直角坐标系对齐，获得局部平面直角坐标系上的点投影至点云地图坐标系的旋转变换矩阵r0与平移变换矩阵t0，
11.利用公式：
[0012][0013]
获得所述的相应点投影至点云地图的点，其中p
gps
代表所述的相应点，p’gps
表示所述的相应点投影至点云地图的点。
[0014]
进一步，所述的一种基于多传感器融合的机器人重定位方法中通过九轴imu获取实时方向角信息，包括：
[0015]
通过导入实时九轴imu偏航角，利用所述偏航角信息构成实时重定位位姿。
[0016]
进一步，所述的一种基于多传感器融合的机器人重定位方法中判断点云配准定位是否失败，包括：
[0017]
通过点云地图输出配准得分score，与设置的配准得分阈值score0进行比较，当score>score0时，判定为点云配准定位失败，利用实时重定位位姿纠正实时点云位姿。
[0018]
一种基于多传感器融合的机器人重定位系统，包括分析映射模块和重定位模块，
[0019]
分析映射模块通过激光雷达加载点云地图，标记地图坐标原点，
[0020]
通过单天线gps以地图坐标原点处经纬度坐标为原点，遵循wgs84坐标系统准则，建立局部平面直角坐标系，
[0021]
获取点云地图原点相对于局部平面直角坐标系的位姿，其中单天线gps提供三轴位置信息，九轴imu提供方向信息，
[0022]
重定位模块通过点云地图进行点云配准，对于点云配准定位失败的情况，通过单天线gps实时导入gps经纬度信息，将经纬度信息映射至局部平面直角坐标系获得相应点并将所述相应点投影至点云地图获取实时姿态信息，通过九轴imu获取实时方向角信息，利用实时姿态信息和实时方向角信息构成实时重定位位姿，利用实时重定位位姿p
c
纠正实时点云位姿，重新定位。
[0023]
进一步，所述的一种基于多传感器融合的机器人重定位系统中分析映射模块将所述的相应点投影至点云地图的过程，包括：
[0024]
通过点云地图与局部平面直角坐标系对齐，获得局部平面直角坐标系上的点投影至点云地图坐标系的旋转变换矩阵r0与平移变换矩阵t0，
[0025]
利用公式：
[0026][0027]
获得所述的相应点投影至点云地图的点，其中p
gps
代表所述的相应点，p’gps
表示所述的相应点投影至点云地图的点。
[0028]
进一步，所述的一种基于多传感器融合的机器人重定位系统中重定位模块通过九轴imu获取实时方向角信息的过程，包括：
[0029]
通过导入实时九轴imu偏航角，利用所述偏航角构成实时重定位位姿。
[0030]
进一步，所述的一种基于多传感器融合的机器人重定位系统中重定位模块判断点云配准定位是否失败的过程，包括：
[0031]
通过点云地图输出配准得分score，与设置的配准得分阈值score0进行比较，当score>score0时，判定为点云配准定位失败，利用实时重定位位姿纠正实时点云位姿。
[0032]
一种基于多传感器融合的机器人重定位装置，包括：至少一个存储器和至少一个处理器；
[0033]
所述至少一个存储器，用于存储机器可读程序；
[0034]
所述至少一个处理器，用于调用所述机器可读程序，执行所述的一种基于多传感器融合的机器人重定位方法。
[0035]
本发明的有益之处是：
[0036]
本发明提供一种基于多传感器融合的机器人重定位方法，主要涉及单天线gps、九
轴imu与激光雷达三种传感器进行机器人重定位。主要解决机器人依靠激光雷达点云帧与点云地图进行配准定位时出现的定位丢失问题，同时也可解决开始配准时机器人无法自动定位的问题。其中单天线gps模块主要提供重定位所需的三轴坐标信息，九轴imu主要提供方向角信息，此外，为了将gps输出的经纬度坐标信息转化到点云地图坐标系，本发明还利用到了wgs84坐标系统准则。相比于现有技术中依靠rtk或者双天线gps进行重定位的方法，本方法在有效稳定完成重定位功能的同时，具有更低的价格成本。
附图说明
[0037]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0038]
图1是本发明方法流程示意图。
[0039]
图2是激光雷达与九轴imu坐标系示意图。
具体实施方式
[0040]
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
[0041]
本发明提供一种基于多传感器融合的机器人重定位方法，通过激光雷达加载点云地图，标记地图坐标原点，
[0042]
通过单天线gps以地图坐标原点处经纬度坐标为原点，遵循wgs84坐标系统准则，建立局部平面直角坐标系，
[0043]
获取点云地图原点相对于局部平面直角坐标系的位姿，其中单天线gps提供三轴位置信息，九轴imu提供方向信息，
[0044]
通过点云地图进行点云配准，对于点云配准定位失败的情况，通过单天线gps实时导入gps经纬度信息，将经纬度信息映射至局部平面直角坐标系获得相应点并将所述相应点投影至点云地图获取实时姿态信息，通过九轴imu获取实时方向角信息，利用实时姿态信息和实时方向角信息构成实时重定位位姿，利用实时重定位位姿pc纠正实时点云位姿，重新定位。
[0045]
本发明方法结合单天线gps能够提供位置信息、九轴imu可提供较为可靠的方向信息特点，综合利用单天线gps的位置信息与九轴imu的偏航角信息来完成室外机器人重定位功能，不仅可以保证有效地功能实现，而且具有更低的价格成本，适合批量化生产。
[0046]
具体应用中，在本发明的一些实施例中进行基于多传感器融合的机器人重定位，主要涉及传感器包括激光雷达、gps单天线、九轴imu，激光雷达主要用于点云地图的创建与配准定位，gps单天线用于获取机器人实时的位置信息，九轴imu主要用于获取机器人的三轴姿态角信息。其中预先固定激光雷达、九轴imu与gps天线，为使九轴imu输出的角度能够更为准确反映出激光雷达的旋转角度，需要九轴imu与激光雷达三轴坐标系分别平行，九轴imu三个坐标轴可表示为(x
imu
，y
imu
，z
imu
)，激光雷达三个坐标轴可表示为(x
lidar
，y
lidar
，z
lidar
)，x
imu
平行于x
lidar
，y
imu
平行于y
lidar
，z
imu
平行于z
lidar
，参考图2。
[0047]
具体如下：
[0048]
通过激光雷达采集的数据输入点云地图，标记地图原点位置，并通过机载gps获取地图原点经纬度坐标信息，gps精度精确到分；例如：北纬36度40分，东经117度13分；
[0049]
以地图原点处经纬度坐标为原点，遵循wgs84坐标系统准则，建立局部平面直角坐标系；
[0050]
依靠点云地图坐标原点的经纬度信息，由于经纬度精度为分，获取点云地图原点在局部平面直角坐标系中的位姿信息，主要表示为：
[0051]
[x0，y0，z0，roll0，pitch0，yaw0]
[0052]
式中，[x0,y0,z0]为地图原点在局部平面坐标系xyz三轴方向上的位置信息。[roll0,pitch0,yaw0]分别对应机体在xyz三轴方向上的反转角度，由九轴imu提供，对应名称为翻滚角、俯仰角、偏航角，
[0053]
在获取点云地图原点在局部平面直角坐标系中的位姿时，同样遵循wgs84坐标系统准则，为了保证获取位置信息的精确性，此时gps经纬度精确到分小数点后八位；
[0054]
加载点云地图并进行点云配准时，输出配准得分score，与设置的配准得分阈值score0进行比较，当score>score0时，判定为点云配准定位失败，
[0055]
对于点云配准定位失败的情况，通过单天线gps实时导入gps经纬度信息，将经纬度信息转化至局部平面直角坐标系，获得相应点，将所述的相应点投影至点云地图，其中所述的相应点投影至点云地图，包括：
[0056]
通过点云地图与局部平面直角坐标系对齐，获得局部平面直角坐标系上的点投影至点云地图坐标系的旋转变换矩阵r0与平移变换矩阵t0，
[0057][0058][0059]
利用公式：
[0060][0061]
获得所述的相应点投影至点云地图的点，其中p
gps
代表所述的相应点，即经纬度信息转化至局部平面直角坐标系，得到点p
gps
，p’gps
表示所述的相应点投影至点云地图的点，即将点p
gps
投影至点云地图坐标系得到点p’gps
；
[0062]
并导入实时九轴imu偏航角yaw0，构成实时重定位位姿p
c
，可表示为：
[0063]
p
c
＝[x
′
，y
′
，z
′
，roll
i
，pitch
i
，yaw
i
]
[0064]
利用实时重定位位姿p
c
纠正实时点云位姿p
l
，则：
[0065]
p
l
＝p
c
[0066]
使得机器人点云帧重新回到地图中的正确位置，并重新定位，完成重定位目的。
[0067]
同时本发明还提供一种基于多传感器融合的机器人重定位系统，包括分析映射模块和重定位模块，
[0068]
分析映射模块通过激光雷达加载点云地图，标记地图坐标原点，
[0069]
通过单天线gps以地图坐标原点处经纬度坐标为原点，遵循wgs84坐标系统准则，建立局部平面直角坐标系，
[0070]
获取点云地图原点相对于局部平面直角坐标系的位姿，其中单天线gps提供三轴位置信息，九轴imu提供方向信息，
[0071]
重定位模块通过点云地图进行点云配准，对于点云配准定位失败的情况，通过单天线gps实时导入gps经纬度信息，将经纬度信息映射至局部平面直角坐标系获得相应点并将所述相应点投影至点云地图获取实时姿态信息，通过九轴imu获取实时方向角信息，利用实时姿态信息和实时方向角信息构成实时重定位位姿，利用实时重定位位姿p
c
纠正实时点云位姿，重新定位。
[0072]
上述系统内的各模块之间的信息交互、执行过程等内容，由于与本发明方法实施例基于同一构思，具体内容可参见本发明方法实施例中的叙述，此处不再赘述。同样地，本发明系统可以结合单天线gps能够提供位置信息、九轴imu可提供较为可靠的方向信息特点，综合利用单天线gps的位置信息与九轴imu的偏航角信息来完成室外机器人重定位功能，不仅可以保证有效地功能实现，而且具有更低的价格成本，适合批量化生产。
[0073]
以及本发明提供一种基于多传感器融合的机器人重定位装置，包括：至少一个存储器和至少一个处理器；
[0074]
所述至少一个存储器，用于存储机器可读程序；
[0075]
所述至少一个处理器，用于调用所述机器可读程序，执行所述的一种基于多传感器融合的机器人重定位方法。
[0076]
上述装置内的处理器的信息交互、执行可读程序过程等内容，由于与本发明方法实施例基于同一构思，具体内容可参见本发明方法实施例中的叙述，此处不再赘述。同样地，本发明装置可以结合单天线gps能够提供位置信息、九轴imu可提供较为可靠的方向信息特点，综合利用单天线gps的位置信息与九轴imu的偏航角信息来完成室外机器人重定位功能，不仅可以保证有效地功能实现，而且具有更低的价格成本，适合批量化生产。
[0077]
需要说明的是，上述较佳实施例中各流程和各系统结构中不是所有的步骤和模块都是必须的，可以根据实际的需要忽略某些步骤或模块。各步骤的执行顺序不是固定的，可以根据需要进行调整。上述各实施例中描述的系统结构可以是物理结构，也可以是逻辑结构，即，有些模块可能由同一物理实体实现，或者，有些模块可能分由多个物理实体实现，或者，可以由多个独立设备中的某些部件共同实现。
[0078]
以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。