机器人的室内定位方法、装置、电子设备及存储介质

1.本技术涉及机器人定位技术领域，特别涉及一种机器人的室内定位方法、装置、电子设备及存储介质。


背景技术：

2.随着农业自动化的发展，农业机器人成为了农业生产中重要的生产力工具。为了提高农业机器人的自动化程度，用户需要解决机器人在农机库中的自动驾驶问题，而自动驾驶的一个基础条件就是机器人的自主定位。
3.相关技术中，常见的定位系统包括：惯性导航系统、无线电信标、二维码信标、视觉里程计、gps(global positioning system，定时测距导航卫星全球定位系统)和多传感器融合技术等。
4.然而，上述系统在工作时往往会出现易受干扰，导致定位不准确，或者单独使用某个系统，定位能力不足等问题，亟待解决。


技术实现要素：

5.本技术提供一种机器人的室内定位方法、装置、电子设备及存储介质，以解决机器人在具有相似场景的室内环境全局定位困难的问题，实现了机器人在室内的有地图条件下的快速重定位。
6.为达到上述目的，本技术第一方面实施例提出一种机器人的室内定位方法，包括以下步骤：
7.获取机器人的编码器数据、惯性测量单元imu(inertial measurement unit)数据、雷达数据和视频数据；
8.根据所述编码器数据和所述imu数据得到里程测量数据，并根据所述里程测量数据和预设的机器人运动学模型，得到每个粒子当前时刻的先验位姿数据；以及
9.根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，并根据所述先验位姿数据和所述每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，并将所述权重最高的一族粒子的平均位姿作为所述机器人的位姿估计值，且根据所述机器人的位姿估计值得到所述机器人的室内定位。
10.根据本技术的一个实施例，根据所述先验位姿数据和所述每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，包括：
11.基于所述每个粒子权重的实际估计值，识别权重的实际估计值小于第一预设阈值的待剔除粒子；
12.剔除所述待剔除粒子，并在剩余粒子的预设范围内散布满足预设散布条件的目标粒子，且在预设时长后根据所述先验位姿数据、所述目标粒子的散布结果和剩余粒子权重的实际估计值得到所述权重最高的一族粒子的平均位姿。
13.根据本技术的一个实施例，所述根据所述雷达数据和所述视频数据计算每个粒子
权重的实际估计值，包括：
14.基于预设的权重估计公式，根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，其中，所述预设的权重估计公式为：
15.w＝w
l
×
wc；
16.其中，w为所述粒子权重的实际估计值；w
l
为利用所述雷达数据计算的粒子权重；wc为利用所述视频数据计算的粒子权重。
17.根据本技术的一个实施例，所述视频数据估计的粒子权重为：
18.wc＝w
cd
(d)
×wcα
(α)；
19.其中，其中，
20.根据本技术的一个实施例，所述编码器数据包括第一编码器读取数据和第二编码器读取数据，在根据所述编码器数据和所述imu数据得到所述里程测量数据之前，还包括：
21.根据所述第一编码器读取数据和所述第二编码器读取数据的平均值得到所述编码器数据。
22.根据本技术的一个实施例，上述的机器人的室内定位方法，还包括：
23.基于所述视频数据对应的二维码信标，从信标编号表查找得到所述视频数据在地图坐标系的位置。
24.根据本技术实施例提出的机器人的室内定位方法，通过根据机器人的编码器数据和imu数据得到里程测量数据，并将里程测量数据和预设的机器人运动学模型进行估计，得到每个粒子当前时刻的先验位姿数据，根据雷达数据和视频数据计算每个粒子权重的实际估计值，并将根据先验位姿数据和每个粒子权重的实际估计值得到的权重最高的一族粒子的平均位姿，作为机器人的位姿估计值，且根据所述机器人的位姿估计值得到所述机器人的室内定位。由此，解决了机器人在具有相似场景的室内环境全局定位困难的问题，实现了机器人在室内的有地图条件下的快速重定位。
25.为达到上述目的，本技术第二方面实施例提出一种机器人的室内定位装置，包括：
26.获取模块，用于获取机器人的编码器数据、惯性测量单元imu数据、雷达数据和视频数据；
27.结果模块，用于根据所述编码器数据和所述imu数据得到里程测量数据，并根据所述里程测量数据和预设的机器人运动学模型，得到每个粒子当前时刻的先验位姿数据；以及
28.定位模块，用于根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，并根据所述先验位姿数据和所述每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，并将所述权重最高的一族粒子的平均位姿作为所述机器人的位姿估计值，且根据所述机器人的位姿估计值得到所述机器人的室内定位。
29.根据本技术的一个实施例，所述定位模块，具体用于：
30.基于所述每个粒子权重的实际估计值，识别权重的实际估计值小于第一预设阈值
的待剔除粒子；
31.剔除所述待剔除粒子，并在剩余粒子的预设范围内散布满足预设散布条件的目标粒子，且在预设时长后根据所述先验位姿数据、所述目标粒子的散布结果和剩余粒子权重的实际估计值得到所述权重最高的一族粒子的平均位姿。
32.根据本技术的一个实施例，所述定位模块，具体用于：
33.基于预设的权重估计公式，根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，其中，所述预设的权重估计公式为：
34.w＝w
l
×
wc；
35.其中，w为所述粒子权重的实际估计值；w
l
为利用所述雷达数据计算的粒子权重；wc为利用所述视频数据计算的粒子权重。
36.根据本技术的一个实施例，所述视频数据估计的粒子权重为：
37.wc＝w
cd
(d)
×wcα
(α)；
38.其中，其中，
39.根据本技术的一个实施例，所述编码器数据包括第一编码器读取数据和第二编码器读取数据，在根据所述编码器数据和所述imu数据得到所述里程测量数据之前，所述结果模块，还用于：
40.根据所述第一编码器读取数据和所述第二编码器读取数据的平均值得到所述编码器数据。
41.根据本技术的一个实施例，上述的机器人的室内定位装置，还包括：
42.查找模块，用于基于所述视频数据对应的二维码信标，从信标编号表查找得到所述视频数据在地图坐标系的位置。
43.根据本技术实施例提出的机器人的室内定位装置，通过根据机器人的编码器数据和imu数据得到里程测量数据，并将里程测量数据和预设的机器人运动学模型进行估计，得到每个粒子当前时刻的先验位姿数据，根据雷达数据和视频数据计算每个粒子权重的实际估计值，并将根据先验位姿数据和每个粒子权重的实际估计值得到的权重最高的一族粒子的平均位姿，作为机器人的位姿估计值，且根据所述机器人的位姿估计值得到所述机器人的室内定位。由此，解决了机器人在具有相似场景的室内环境全局定位困难的问题，实现了机器人在室内的有地图条件下的快速重定位。
44.为达到上述目的，本技术第三方面实施例提出一种电子设备，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如上述实施例所述的机器人的室内定位方法。
45.为达到上述目的，本技术第四方面实施例提出一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行，以用于实现如上述实施例所述的机器人的室内定位方法。
46.本技术附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变
得明显，或通过本技术的实践了解到。
附图说明
47.本技术上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
48.图1为根据本技术一个实施例的orb-slam正常工作环境运行结果示意图；
49.图2为根据本技术一个实施例的相似楼道环境模拟实验示意图；
50.图3为根据本技术一个实施例的仅使用雷达数据的室内机器人运行过程中粒子分布情况示意图；
51.图4为根据本技术实施例提供的一种机器人的室内定位方法的流程图；
52.图5为根据本技术一个实施例的机器人的室内定位系统的组成示意图；
53.图6为根据本技术一个实施例的转向模型示意图；
54.图7为根据本技术一个实施例的根据视频数据估计的位置分布情况示意图；
55.图8为根据本技术一个实施例的根据视频数据估计的偏航角分布情况示意图；
56.图9为根据本技术一个实施例的粒子权重随距离变化曲线的示意图；
57.图10为根据本技术一个实施例的二维码信标示意图；
58.图11为根据本技术一个实施例的地图、摄像头和机体坐标系的空间关系示意图；
59.图12为根据本技术一个实施例的机器人在地图中的各位姿分布示意图；
60.图13为根据本技术一个实施例的使用摄像头时的估计位姿变化曲线的示意图；
61.图14为根据本技术一个实施例的为使用摄像头时的估计位姿变化曲线的示意图；
62.图15为根据本技术实施例的机器人的室内定位装置的方框示意图；
63.图16为根据本技术实施例的电子设备的结构示意图。
具体实施方式
64.下面详细描述本技术的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本技术，而不能理解为对本技术的限制。
65.下面参照附图描述根据本技术实施例提出的机器人的室内定位方法、装置、电子设备及存储介质，首先将参照附图描述根据本技术实施例提出的机器人的室内定位方法。
66.在介绍本技术实施例提出的机器人的室内定位方法之前，先简单介绍下相关技术中常见的定位系统：
67.(1)惯性导航系统，是利用加速度计、陀螺仪、编码器等惯性测量元件或速度测量元件，测量机器人相对初始点的位移。
68.(2)rfid(radio frequency identification，无线通信技术)、wifi(wireless fidelity，移动热点)、蓝牙、uwb(ultra wide band，超宽带)等是被广泛应用于定位的无线电技术，这些技术通常需要在空间内放置多个发送或接受无线电的信标，机器人根据信标信号强弱、与信标的通信时间等确定和信标之间的距离，再根据机器人和多个信标的距离以及信标在地图上的分布情况求解机器人位置。
69.(3)二维码信标，相较于利用电磁信号的信标具有不受电磁干扰、成本低的优点，
定位时只需要一个信标即可，可以喷涂或者张贴在需要标识的位置，易于布置。机器人可以根据摄像头拍摄的二维码信标图像估计自身与信标之间的位置关系，再根据信标在地图上的张贴位置确定自身在地图上的位置。
70.(4)视觉里程计，主要依靠视觉确定机器人在环境中的位姿，视觉里程计的核心思想是通过比较相邻两帧图像的差异，获得在此间隔中摄像头的位移，以此得到摄像头的位姿。
71.(5)gps，是室外定位最常用的定位技术，它通过接收器与多个卫星之间的距离信息确定接收器的位置。
72.(6)激光雷达，是利用激光扫描周围环境，通过测量激光反射时间确定雷达和周围环境的位置信息，利用粒子滤波等方案确定自身在地图上的位置。激光雷达在扫地机器人等机器人上应用广泛，激光雷达测量精度通常较高。
73.(7)多传感器融合技术，将多个传感器的测量数据通过算法整合应用，以此发挥多个传感器的优点。例如激光雷达和gps融合，可以帮助机器人在gps信号受限的位置实现高精度定位，在京东室外物流车、各种自动驾驶中比较常见；摄像头和激光雷达融合，可以让机器人获得同时具有深度信息和纹理信息的数据，在一些特殊场景例如勘探、路口行车中有广泛应用。
74.然而，上述常见的定位系统存在的现有问题如下：
75.(1)由于存在累计误差，惯性导航系统受测量误差的影响较大，并且测量值是相对初始点的位置而非在地图中的绝对位置。
76.(2)无线电信标通常用于室内定位，容易受到电磁波、障碍物等的干扰，并且只能在信标信号覆盖的范围内定位。
77.(3)二维码信标只有在信标出现在视野中并且拍摄清晰时才能用于定位，所以通常不能被单独使用。
78.(4)视觉里程计在环境中纹理信息较少时会失效，例如在写字楼走廊中，视野范围内可能都是白色墙壁，导致视觉里程计无法正常工作。
79.本技术对svo(fast semi-direct monocular visual odometry，半直接视觉里程计)和orb-slam(oriented fast and rotated brief-simultaneous localization and mapping)两个常用视觉里程计算法进行了实验，svo算法通过识别图像中的特征点，比较特征点的位移来判断摄像头的位移；orb-salm在识别特征点的基础上还会创建局部点云地图。
80.具体而言，在对svo的仿真和实物实验中发现，svo更适合用于飞行器摄像头朝地面拍摄图像时的定位。svo通过比较相邻两帧的图像，可以获得摄像头在与摄像头图像平面平行的平面上的平动，沿此平面法向量的平动和绕此平面法向量的转动，即摄像头朝下的飞行器三个方向的平动与偏航，但是摄像头朝向车头方向的移动机器人，其旋转轴是与摄像头图像平面平行的向量，在机器人转弯过程中特征点很容易丢失，由于svo只比较相邻图像，在机器人旋转之后便失去了定位能力。
81.orb-slam会在定位的同时创建点云地图，保留机器人运动过程中的历史特征点，因此在机器人旋转过程中也能保持较好的定位效果，但是研究发现，orb-slam能成功定位的条件是环境中有足够多的特征点，例如具有较多桌椅的办公室环境，而在实际的走廊环
境中，位于机器人底盘上的摄像头拍摄的图片中大部分位置是墙壁和地面，特征点极少，不足以使orb-slam算法完成定位和建立点云地图。如图1(a)所示，列出实验中截取的orb-slam可以正常工作的办公室环境图像和图1(b)所示的真实的走廊环境图像，图中所示的点线即为orb-slam识别的特征点，当特征点足够多时算法会由线变为点，可以看出在真实走廊环境中除去阴影产生的边缘特征外特征点较少，不足以让算法正常执行。
82.(5)gps无法在室内使用，容易受遮挡影响，且测量精度较低。
83.(6)激光雷达是目前机器人和自动驾驶中最常用的用于定位的传感器之一，由于激光雷达属于主动式传感器，并且精度较高，所以在绝大多数场景下都可以使用，但是激光雷达只能扫描周围的障碍物信息，需要结合对自身位置的先验估计才能实现全局定位，因而只使用激光雷达数据会出现全局定位能力不足的问题。全局定位能力不足主要有两个具体体现：一是机器人被绑架时，即机器人的先验位姿偏离真实位姿较远时，单凭激光雷达数据机器人的估计位姿很难收敛到真实位姿附近；二是在机器人运动过程中机器人估计位姿对真实位姿的跟踪情况不理想。
84.此处以大部分粒子集中在直径为1m的圆形区域内为收敛标准，聚集指粒子散布在半径约1.5m的范围内。在室内室外情境产生上述问题的具体原因不同，具体分析如下：
85.在室内场景中可能具有很多相似环境，例如相似的走廊、转角等，当先验位姿距离真实位姿较远时，激光雷达数据和当前先验位姿周围的环境匹配度也可能较高，粒子会收敛在错误位置，本技术在移动机器人平台上对算法进行实验和验证，选取如图2所示的楼道环境进行了模拟实验，机器人运行过程中粒子分布情况如图3所示，图3(a)、(b)、(c)分别表示机器人初始状态、运行20秒后、运行2分30秒后的粒子分布示意图，图中所示1位姿(用于定位的粒子滤波器中的粒子)聚集的区域中心为机器人对自身位置的估计，实验中机器人真实位姿为图3所示的2位姿，粒子初始聚集区域为1位姿附近，机器人在2位姿附近做前后往复运动，以获取更多地图信息。雷达探测到的实际环境为“t”字形走廊，和1位姿的长直走廊具有一定相似性，且绝大部分粒子初始位姿在长直走廊处，所以粒子收敛到了1位姿，并且可以看出粒子并没有重定位到2位姿的趋势，重复上述实验5次，发现结果相同。
86.此外，室内场景环境范围小，地图内参照物较多，雷达获得的信息较多，所以在雷达收敛到真实位姿附近之后，在机器人运动过程中能跟随真实位姿，但是雷达只获取二维信息，当真实位姿在粒子聚集区域内时，估计位姿能收敛到真实位姿，但是速度较慢。
87.正是基于上述问题，本技术提出一种机器人的室内定位方法，通过根据机器人的编码器数据和imu数据得到里程测量数据，并将里程测量数据和预设的机器人运动学模型进行估计，得到每个粒子当前时刻的先验位姿数据，根据雷达数据和视频数据计算每个粒子权重的实际估计值，并将根据先验位姿数据和每个粒子权重的实际估计值得到的权重最高的一族粒子的平均位姿，作为机器人的位姿估计值，且根据所述机器人的位姿估计值得到所述机器人的室内定位。由此，解决了机器人在具有相似场景的室内环境全局定位困难的问题，实现了机器人在室内的有地图条件下的快速重定位。
88.图4是本技术一个实施例的机器人的室内定位方法的流程图。
89.在本技术实施例中，选用编码器、惯性测量单元imu、激光雷达和摄像头四种传感器，基于ros(robot operating system，机器人操作系统)系统、apriltags二维码信标、扩展卡尔曼滤波算法和amcl(adaptive monte carlo localization，自适应蒙特卡洛定位)
算法等做传感器数据融合，该系统组成如图5所示。
90.具体而言，图5为本技术实施例的机器人的室内定位系统，主要包括粒子滤波器和各个传感器，利用扩展卡尔曼滤波器(extended kalman filter，简称ekf)融合编码器数据和imu数据，得到里程测量数据，用以估计机器人相对初始点的位姿(位置和姿态，即二维位置和航向)，粒子滤波器采用自适应蒙特卡洛算法实现粒子滤波，其核心思想是在地图坐标系内散布一些带有位姿信息的粒子，根据里程测量数据和预设的机器人运动学模型，估计得到每个粒子当前时刻的先验位姿数据，然后根据其他传感器的数据和模型计算每个粒子的权重，删除权重较低的粒子，并在权重较高的粒子附近散布更多粒子，这样经过一段时间后所有粒子都会聚集在实际位姿附近。
91.其中，amcl算法的自适应体现在：当粒子散布范围较广时，可以散布较多的目标粒子，而在粒子开始集中后，只需要散布较少目标粒子，以提高运行速度；另一方面，amcl在绝大多数粒子已经聚集在一个位置附近时，仍会在地图其他位置放置一些目标粒子，由此，在机器人因为某些原因突然改变位置(机器人绑架)时能够实现重定位。
92.需要说明的是，确定是否需要重定位的依据是长期似然和短期似然函数，长期似然函数即为在较长时间内粒子权重的均值，短期似然则是较短时间内粒子权重的均值，在机器人正常运行时长期似然和短期似然大致相等，而机器人被绑架到其他位置时短期似然突然降低，且远小于长期似然，则表明机器人需要重定位。
93.该算法将权重最高的一族粒子的平均位姿作为机器人的位姿估计值。
94.进一步地，本技术实施例还建立了地图坐标系、机体坐标系、里程计坐标系和传感器坐标系。
95.其中，地图坐标系(又称为map坐标系)即环境的坐标系，zm轴垂直地面向上。
96.机体坐标系(即base_link坐标系)是与机器人底盘固连的非惯性系，坐标原点ob为机器人底盘的几何中心在地面上的投影位置，xb轴指向车头，zb轴垂直于底盘方向向上，yb轴垂直于x
b-o
b-zb平面向左。机器人的定位实际上就是通过处理各种传感器数据确定机体坐标系在地图坐标系中的相对位置。
97.里程计坐标系(即odom坐标系)的引入是为了确定机器人在地图中的位姿，作为确定机体坐标系和地图坐标系之间关系的桥梁，odom坐标系的原点oo是移动机器人开始运动时的位置，xo轴与初始状态时的机体坐标系xb轴重合，yo轴与初始状态时的机体坐标系yb轴重合。
98.传感器坐标系是各个传感器自身的坐标系，传感器返回的原始位置和姿态信息是在传感器自身坐标系中的。本技术实施例涉及到的传感器坐标系包括imu坐标系、雷达坐标系、gps坐标系和摄像头坐标系，编码器获取的原始数据只是轮子前进的距离，并且和机体坐标系绑定，因此，没有单独的编码器坐标系。
99.如图4所示，该机器人的室内定位方法包括以下步骤：
100.在步骤s401中，获取机器人的编码器数据、惯性测量单元imu数据、雷达数据和视频数据。
101.其中，编码器数据、惯性测量单元imu数据、雷达数据和视频数据分别取自编码器、imu、激光雷达和摄像头四种传感器，本技术实施例可以利用相关技术手段获取这四种传感器数据，为避免冗余，此处不再赘述。
102.具体而言，惯性测量单元集成了加速度计和陀螺仪，用于测量三轴姿态角(或角速率)以及加速度；激光雷达是主动式传感器，通过发射激光和接受反射信号来测量周围环境的深度信息，工作时雷达360度旋转，根据设定转速和角度分辨率，返回一组极坐标数据，表示雷达坐标系下二位平面内若干方向上的深度信息，雷达周而复始地旋转，实时采集周围环境信息，每旋转一周可以向嵌入式计算平台发送一帧数据；单目摄像头传感器主要是用于拍摄二维图像，利用视频数据辅助机器人定位。
103.在步骤s402中，根据编码器数据和imu数据得到里程测量数据，并根据里程测量数据和预设的机器人运动学模型，得到每个粒子当前时刻的先验位姿数据。
104.进一步地，在一些实施例中，编码器数据包括第一编码器读取数据和第二编码器读取数据，在根据编码器数据和imu数据得到里程测量数据之前，还包括：根据第一编码器读取数据和第二编码器读取数据的平均值得到编码器数据。
105.具体而言，本技术实施例选用了双编码器，即在机器人两个后轮处均装有编码器，分别用来测量两个后轮的单位时间里程，因此，取两轮编码器读取数据的平均值作为实际里程的观测值(即最终的编码器数据)，设左轮编码器读取的里程数据为d1，右轮编码器读取的里程数据为d2，则机器人里程增加为：
[0106][0107]
进一步地，本技术实施例使用的机器人可以认为在转弯时四个车轮具有相同的旋转中心，设转弯过程中外侧后轮转弯半径为r2，里程计数据为d2，内侧后轮转弯半径为r1，里程计数据为d1，机器人转过的角度为ψ，如图6所示，则有：
[0108][0109]
可解得：
[0110][0111]
设采样周期为δt，机器人速度和角速度的测量值为：
[0112][0113][0114]
在步骤s403中，根据雷达数据和视频数据计算每个粒子权重的实际估计值，并根据先验位姿数据和每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，并将权重最高的一族粒子的平均位姿作为机器人的位姿估计值，且根据机器人的位姿估计值得到机器人的室内定位。
[0115]
可以理解的是，本技术实施例可以根据雷达数据和视频数据计算得到每个粒子权
重的实际估计值，再将根据先验位姿数据和每个粒子权重的实际估计值得到的权重最高的一族粒子的平均位姿，作为机器人的位姿估计值，由此，得到机器人的室内定位。
[0116]
进一步地，在一些实施例中，根据先验位姿数据和每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，包括：基于每个粒子权重的实际估计值，识别权重的实际估计值小于第一预设阈值的待剔除粒子；剔除待剔除粒子，并在剩余粒子的预设范围内散布满足预设散布条件的目标粒子，且在预设时长后根据先验位姿数据、目标粒子的散布结果和剩余粒子权重的实际估计值得到权重最高的一族粒子的平均位姿。
[0117]
具体而言，本技术实施例可以根据雷达数据和视频数据计算每个粒子权重的实际估计值，将权重的实际估计值小于第一预设阈值的粒子识别为待剔除粒子，剔除权重较低的待剔除粒子，并在剩余的权重较高的粒子的预设范围内散布满足预设散布条件的目标粒子，经过一段时间后，所有粒子都会聚集在实际位姿附近，从而根据先验位姿数据、目标粒子的散布结果和剩余粒子权重的实际估计值得到权重最高的一族粒子的平均位姿。
[0118]
进一步地，在一些实施例中，根据雷达数据和视频数据计算每个粒子权重的实际估计值，包括：基于预设的权重估计公式，根据雷达数据和视频数据计算每个粒子权重的实际估计值，其中，预设的权重估计公式为：
[0119]
w＝w
l
×
wc；
[0120]
其中，w为粒子权重的实际估计值，w
l
为利用雷达数据计算的粒子权重，wc为利用视频数据计算的粒子权重。
[0121]
具体而言，本技术实施例使用现有激光雷达传感器模型似然域模型，根据地图上距离一束激光端点最近的障碍物与激光端点的距离为依据，估计这束激光的概率，最后对所有激光的概率加和作为对应粒子的权重。事实上就是比较激光扫描的周围环境轮廓与粒子先验位姿在地图中的周围环境轮廓，若匹配程度高，该粒子权重就高。
[0122]
其中，在一些实施例中，视频数据估计的粒子权重为：
[0123]
wc＝w
cd
(d)
×wcα
(α)；
[0124]
其中，其中，
[0125]
具体而言，摄像头传感器模型的任务是确定wc与视频数据估计位姿和粒子先验位姿之间偏差的函数关系。本技术实施例中，将wc分为两部分，一部分为位置偏差的函数w
cd
，一部分为角度偏差的函数w
cα
，机器人在二维平面不同方向的位置是同性的，所以位置偏差函数取估计位置和先验位置的距离d为自变量，角度偏差函数取估计位置和先验位置的角度差α为自变量。
[0126]
本技术实施例对机器人在固定一点根据视频数据估计的位姿进行了记录，结果如图7和图8所示，其中，图7(a)为数据分布图，用于估计位姿在二维平面中的分布情况，图7(b)为频次分布图，用于显示某一个估计位姿在所有数据中出现的频次，图8为频率-偏航角分布图。
[0127]
由此，可以看出根据视频数据估计的位姿在真实位姿0.03m距离和0.02弧度角度
偏差内基本服从均匀分布。
[0128]
若取：
[0129]wcd
(d)＝1d≤0.03m；
[0130]
可以发现，为了保证粒子滤波算法的正常运行，先验位置与摄像头估计位置距离超过1m的粒子权重不能过小，否则由于计算精度等因素影响，当粒子初始聚集范围不包括真实位置时，算法中所有粒子的权重都在零附近，算法便不能根据粒子权重计算得到真实位姿。根据实验测量，在距离超过0.03m范围内增加0.05的权重，可以保证算法正常运行，并对粒子收敛没有显著影响，另外，实验中对二维码信标在地图中的估计存在一定误差，所以将函数分段点的距离值取为0.1m。
[0131]
此时：
[0132][0133]
然而，由于d》0.1m的粒子间权重没有差异，即无法区分粒子先验位置距离摄像头估计的远近，当粒子初始聚集区域不包含摄像头估计位置时，粒子权重只是乘上了相同系数，仍无法收敛到利用摄像头数据估计的真实位置，因此，在0.1m《d≤d1范围内加入了一段单调递减的线性区域，即：
[0134][0135]
其中，d1取值过小不能解决上述问题，d1取值过大导致线性递减段过于平缓，同样不能显著区分粒子权重，根据实验观测，取d1＝3m时，粒子能有效收敛，此时，粒子权重随距离变化曲线图如图9所示。
[0136]
角度偏差的估计中同样存在上述问题，因此选用正态分布表示角度偏差，取：
[0137]wcα
(α)＝0.2
×fn
(α)+0.1n～n(0,0.2)；
[0138]
其中，fn(α)表示角度偏差α的粒子的概率密度，取角度分度值为0.2弧度，相乘即得概率。
[0139]
本技术实施例在实验中发现，由于角度偏差α的范围为-π到π，而距离偏差d的范围为0到正无穷，在实验环境中也可达10m以上，若要同步估计角度和距离偏差，那么位置偏差较大的粒子会优先收敛到正确的方向上，而此时算法会删除位置正确但方向错误的粒子，最终所有粒子均收敛到方向正确但位置错误的位姿上，因此，本技术实施例采取优先收敛位置的策略，当粒子先验位姿距离根据摄像头数据估计的位姿距离d≤2m时开始根据角度计算偏差，于是最终取：
[0140][0141]
进一步地，在一些实施例中，上述的机器人的室内定位方法，还包括：基于视频数据对应的二维码信标，从信标编号表查找得到视频数据在地图坐标系的位置。
[0142]
具体而言，本技术实施例选用摄像头识别的如图10所示的april tags二维码信标的方法来利用摄像头信息得到机器人位姿，其中，图片下方文字最后的编号即为二维码所
包含的信息，表示此信标的编号，每个信标在地图坐标系中的位姿可以通过信标编号查表得到，在农机库中，可以将二维码张贴在机库入口、每个泊位的墙面上，以辅助机器人定位。
[0143]
进一步地，本技术实施例通过调用apriltag_ros算法包，可以从摄像头拍摄的图像中识别到二维码，根据信标编号和摄像头内参矩阵，利用pnp方法获得与地图固连的二维码相对于摄像头的位姿，根据二维码在地图中的位姿可以确定摄像头坐标系和地图坐标系的转换关系，再根据摄像头和机器人的位姿关系，即可确定机器人在地图中的位姿。
[0144]
其中，地图、摄像头和机体坐标系的空间关系如图11所示，摄像头坐标系相对机体坐标系存在旋转，摄像头坐标系的x轴和y轴表示的是摄像头照片坐标，设地图坐标系到摄像头坐标系的三维旋转矩阵为r1，位移向量为t1，摄像头坐标系到机体坐标系的旋转矩阵为r0，机体坐标系在摄像头坐标系中的位移向量为t0，记空间中一点在摄像头坐标系中的位置为pc，在地图坐标系中的位置为pm，在机体坐标系中的位置为pb，则有：
[0145]
pc＝r1pm+t1；
[0146]
pb＝r0(p
c-t0)；
[0147]
因此，机体坐标系在地图中的位置为：
[0148][0149]
机体坐标系在地图坐标系中的旋转矩阵为：
[0150][0151]
为便于本领域技术人员进一步了解本技术实施例提出的机器人的室内定位方法，下面结合示例做进一步说明。
[0152]
如图12所示，图12为根据本技术的一个实施例的机器人在地图中的各位姿分布示意图，选取初始粒子聚集位姿为1位姿，分别将机器人放置在方向不同的2、3、4三个相似位姿处，重复机器人绑架问题的室内定位实验，融合摄像头数据后真实位姿不在粒子聚集范围内的实验结果如表1所示。
[0153]
表1
[0154][0155]
表中“——”表示粒子在实验中未收敛且没有收敛趋势。
[0156]
图13和图14分别为某次实验中使用摄像头时的估计位姿变化曲线和未使用摄像头时的估计位姿变化曲线，其中，深色曲线为根据摄像头数据估计的位姿，浅色曲线为粒子云表示的位姿，由于摄像头通过识别二维码确定位姿，具有较高的全局定位精度，因此，以根据摄像头数据估计的位姿为真实位姿。由此，在使用摄像头数据后，机器人在相似环境多的楼道场景中具有了重定位能力，角度的收敛能力略弱于位置收敛能力，同时结果显示在距离和朝向不同的位置机器人的重定位能力不同，最差的情况为角度很难收敛。
[0157]
此外，本技术实施例还对机器人运动过程中真实位姿在粒子聚集范围内的情况进行了实验，对比了单纯使用激光雷达数据和同时使用激光雷达和摄像头数据的定位结果，在此实验中角度和位置基本同时收敛，所以不单独列写，真实位姿在粒子聚集范围内的实验结果如表2所示。
[0158]
表2
[0159]
[0160][0161]
从表中可以看出，真实位姿在粒子聚集范围内时，只使用雷达数据在楼道环境可以收敛到真实位姿，但是速度慢于融合摄像头数据之后的实验结果，由此证明了数据融合的优势。
[0162]
综上，本技术实施例利用摄像头识别二维码的方式对现有的使用激光雷达实现定位的方法进行了扩展，针对机器人需要在跨室内外环境中运行的问题，使用了基于粒子滤波的传感器融合框架，便于扩展gps等其他传感器。
[0163]
需要说明的是，本技术实施例涉及到的里程计部分可以用任意惯性原件代替，april tags二维码信标亦可用其他二维码或纹理特征代替，激光雷达也可以用单线、多线等各个种类代替，此处不做唯一限定。
[0164]
根据本技术实施例提出的机器人的室内定位方法，通过根据机器人的编码器数据和imu数据得到里程测量数据，并将里程测量数据和预设的机器人运动学模型进行估计，得到每个粒子当前时刻的先验位姿数据，根据雷达数据和视频数据计算每个粒子权重的实际估计值，并将根据先验位姿数据和每个粒子权重的实际估计值得到的权重最高的一族粒子的平均位姿，作为机器人的位姿估计值，且根据机器人的位姿估计值得到机器人的室内定位。由此，解决了机器人在具有相似场景的室内环境全局定位困难的问题，实现了机器人在室内的有地图条件下的快速重定位。
[0165]
其次参照附图描述根据本技术实施例提出的机器人的室内定位装置。
[0166]
图15是本技术一个实施例的机器人的室内定位装置的方框示意图。
[0167]
如图15所示，该机器人的室内定位装置10包括：获取模块100、结果模块200和定位模块300。
[0168]
其中，获取模块100，用于获取机器人的编码器数据、惯性测量单元imu数据、雷达数据和视频数据；
[0169]
结果模块200，用于根据编码器数据和imu数据得到里程测量数据，并根据里程测量数据和预设的机器人运动学模型，得到每个粒子当前时刻的先验位姿数据；以及
[0170]
定位模块300，用于根据雷达数据和视频数据计算每个粒子权重的实际估计值，并根据先验位姿数据和每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，并将权重最高的一族粒子的平均位姿作为机器人的位姿估计值，且根据机器人的位姿估计值得到机器人的室内定位。
[0171]
进一步地，在一些实施例中，定位模块300，具体用于：
[0172]
基于每个粒子权重的实际估计值，识别权重的实际估计值小于第一预设阈值的待剔除粒子；
[0173]
剔除待剔除粒子，并在剩余粒子的预设范围内散布满足预设散布条件的目标粒子，且在预设时长后根据先验位姿数据、目标粒子的散布结果和剩余粒子权重的实际估计值得到权重最高的一族粒子的平均位姿。
[0174]
进一步地，在一些实施例中，定位模块300，具体用于：
[0175]
基于预设的权重估计公式，根据雷达数据和视频数据计算每个粒子权重的实际估计值，其中，预设的权重估计公式为：
[0176]
w＝w
l
×
wc；
[0177]
其中，w为粒子权重的实际估计值；w
l
为利用雷达数据计算的粒子权重；wc为利用视频数据计算的粒子权重。
[0178]
进一步地，在一些实施例中，视频数据估计的粒子权重为：
[0179]
wc＝w
cd
(d)
×wcα
(α)；
[0180]
其中，其中，
[0181]
进一步地，在一些实施例中，编码器数据包括第一编码器读取数据和第二编码器读取数据，在根据编码器数据和imu数据得到里程测量数据之前，结果模块200，还用于：
[0182]
根据第一编码器读取数据和第二编码器读取数据的平均值得到编码器数据。
[0183]
进一步地，在一些实施例中，上述的机器人的室内定位装置10，还包括：
[0184]
查找模块，用于基于视频数据对应的二维码信标，从信标编号表查找得到视频数据在地图坐标系的位置。
[0185]
需要说明的是，前述对机器人的室内定位方法实施例的解释说明也适用于该实施例的机器人的室内定位装置，此处不再赘述。
[0186]
根据本技术实施例提出的机器人的室内定位装置，通过根据机器人的编码器数据和imu数据得到里程测量数据，并将里程测量数据和预设的机器人运动学模型进行估计，得到每个粒子当前时刻的先验位姿数据，根据雷达数据和视频数据计算每个粒子权重的实际估计值，并将根据先验位姿数据和每个粒子权重的实际估计值得到的权重最高的一族粒子的平均位姿，作为机器人的位姿估计值，且根据机器人的位姿估计值得到机器人的室内定位。由此，解决了机器人在具有相似场景的室内环境全局定位困难的问题，实现了机器人在室内的有地图条件下的快速重定位。
[0187]
图16为本技术实施例提供的电子设备的结构示意图。该电子设备可以包括：
[0188]
存储器1601、处理器1602及存储在存储器1601上并可在处理器1602上运行的计算机程序。
[0189]
处理器1602执行程序时实现上述实施例中提供的机器人的室内定位方法。
[0190]
进一步地，电子设备还包括：
[0191]
通信接口1603，用于存储器1601和处理器1602之间的通信。
[0192]
存储器1601，用于存放可在处理器1602上运行的计算机程序。
[0193]
存储器1601可能包含高速ram(random access memory，随机存取存储器)存储器，也可能还包括非易失性存储器，例如至少一个磁盘存储器。
[0194]
如果存储器1601、处理器1602和通信接口1603独立实现，则通信接口1603、存储器1601和处理器1602可以通过总线相互连接并完成相互间的通信。总线可以是isa(industry standard architecture，工业标准体系结构)总线、pci(peripheral component，外部设备互连)总线或eisa(extended industry standard architecture，扩展工业标准体系结构)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图16中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
[0195]
可选的，在具体实现上，如果存储器1601、处理器1602及通信接口1603，集成在一块芯片上实现，则存储器1601、处理器1602及通信接口1603可以通过内部接口完成相互间的通信。
[0196]
处理器1602可能是一个cpu(central processing unit，中央处理器)，或者是asic(application specific integrated circuit，特定集成电路)，或者是被配置成实施本技术实施例的一个或多个集成电路。
[0197]
本技术实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上的机器人的室内定位方法。
[0198]
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本技术的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
[0199]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本技术的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不是必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的
技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
[0200]
尽管上面已经示出和描述了本技术的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本技术的限制，本领域的普通技术人员在本技术的范围内可以对上述实施例进行变化、修改、替换和变型。

技术特征：
1.一种机器人的室内定位方法，其特征在于，包括以下步骤：获取机器人的编码器数据、惯性测量单元imu数据、雷达数据和视频数据；根据所述编码器数据和所述imu数据得到里程测量数据，并根据所述里程测量数据和预设的机器人运动学模型，得到每个粒子当前时刻的先验位姿数据；以及根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，并根据所述先验位姿数据和所述每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，并将所述权重最高的一族粒子的平均位姿作为所述机器人的位姿估计值，且根据所述机器人的位姿估计值得到所述机器人的室内定位。2.根据权利要求1所述的方法，其特征在于，根据所述先验位姿数据和所述每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，包括：基于所述每个粒子权重的实际估计值，识别权重的实际估计值小于第一预设阈值的待剔除粒子；剔除所述待剔除粒子，并在剩余粒子的预设范围内散布满足预设散布条件的目标粒子，且在预设时长后根据所述先验位姿数据、所述目标粒子的散布结果和剩余粒子权重的实际估计值得到所述权重最高的一族粒子的平均位姿。3.根据权利要求1所述的方法，其特征在于，所述根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，包括：基于预设的权重估计公式，根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，其中，所述预设的权重估计公式为：w＝w
l
×
w
c
；其中，w为所述粒子权重的实际估计值，w
l
为利用所述雷达数据计算的粒子权重，w
c
为利用所述视频数据计算的粒子权重。4.根据权利要求3所述的方法，其特征在于，所述视频数据估计的粒子权重为：w
c
＝
cd
()
×
w
cα
()；其中，其中，5.根据权利要求3所述的方法，其特征在于，所述编码器数据包括第一编码器读取数据和第二编码器读取数据，在根据所述编码器数据和所述imu数据得到所述里程测量数据之前，还包括：根据所述第一编码器读取数据和所述第二编码器读取数据的平均值得到所述编码器数据。6.根据权利要求1所述的方法，其特征在于，还包括：基于所述视频数据对应的二维码信标，从信标编号表查找得到所述视频数据在地图坐标系的位置。7.一种机器人的室内定位装置，其特征在于，包括：
获取模块，用于获取机器人的编码器数据、惯性测量单元imu数据、雷达数据和视频数据；结果模块，用于根据所述编码器数据和所述imu数据得到里程测量数据，并根据所述里程测量数据和预设的机器人运动学模型，得到每个粒子当前时刻的先验位姿数据；以及定位模块，用于根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，并根据所述先验位姿数据和所述每个粒子权重的实际估计值得到权重最高的一族粒子的平均位姿，并将所述权重最高的一族粒子的平均位姿作为所述机器人的位姿估计值，且根据所述机器人的位姿估计值得到所述机器人的室内定位。8.根据权利要求7所述的装置，其特征在于，所述定位模块，具体用于：基于所述每个粒子权重的实际估计值，识别权重的实际估计值小于第一预设阈值的待剔除粒子；剔除所述待剔除粒子，并在剩余粒子的预设范围内散布满足预设散布条件的目标粒子，且在预设时长后根据所述先验位姿数据、所述目标粒子的散布结果和剩余粒子权重的实际估计值得到所述权重最高的一族粒子的平均位姿。9.根据权利要求7所述的装置，其特征在于，所述定位模块，具体用于：基于预设的权重估计公式，根据所述雷达数据和所述视频数据计算每个粒子权重的实际估计值，其中，所述预设的权重估计公式为：w＝w
l
×
w
c
；其中，w为所述粒子权重的实际估计值；w
l
为利用所述雷达数据计算的粒子权重；w
c
为利用所述视频数据计算的粒子权重。10.根据权利要求9所述的装置，其特征在于，所述视频数据估计的粒子权重为：w
c
＝
cd
()
×
w
cα
()；其中，其中，11.根据权利要求9所述的装置，其特征在于，所述编码器数据包括第一编码器读取数据和第二编码器读取数据，在根据所述编码器数据和所述imu数据得到所述里程测量数据之前，所述结果模块，还用于：根据所述第一编码器读取数据和所述第二编码器读取数据的平均值得到所述编码器数据。12.根据权利要求7所述的装置，其特征在于，还包括：查找模块，用于基于所述视频数据对应的二维码信标，从信标编号表查找得到所述视频数据在地图坐标系的位置。13.一种电子设备，其特征在于，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如权利要求1-6任一项所述的机器人的室内定位方法。14.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器
执行，以用于实现如权利要求1-6任一项所述的机器人的室内定位方法。

技术总结
本申请涉及一种机器人的室内定位方法、装置、电子设备及存储介质，其中，方法包括：获取机器人的编码器数据、惯性测量单元IMU数据、雷达数据和视频数据，根据编码器数据和IMU数据得到里程测量数据，并根据里程测量数据和预设的机器人运动学模型，得到每个粒子当前时刻的先验位姿数据，根据雷达数据和视频数据计算每个粒子权重的实际估计值，并将根据先验位姿数据和每个粒子权重的实际估计值得到的权重最高的一族粒子的平均位姿，作为机器人的位姿估计值，且根据所述机器人的位姿估计值得到所述机器人的室内定位。由此，解决了机器人在具有相似场景的室内环境全局定位困难的问题，实现机器人在室内的有地图条件下的快速重定位。机器人在室内的有地图条件下的快速重定位。机器人在室内的有地图条件下的快速重定位。


技术研发人员：张涛 徐皓天 卓晴 李斌
受保护的技术使用者：清华大学
技术研发日：2023.05.22
技术公布日：2023/8/9