CN105547305B - 一种基于无线定位和激光地图匹配的位姿解算方法 - Google Patents

Abstract

本发明公开了一种基于无线定位和激光地图匹配的位姿解算方法，该方法把无线定位方法与激光扫描定位相结合，实现了一种不依赖于卫星定位***的室内高精度定位方法，该方法同时也实现了对定位设备偏航角的计算。该方法既避免了无线定位精度低的缺点，又避免了激光扫描定位易受环境变化的干扰而发生定位位置跳变的情况。本发明实现的高精度位姿计算方法，可广泛应用于位置服务和移动机器人自主导航领域，是位置服务应用和移动机器人的关键核心技术之一。

Description

一种基于无线定位和激光地图匹配的位姿解算方法

技术领域

本发明涉及室内定位、移动机器人和无人驾驶汽车自主定位导航技术领域，具体涉及一种基于无线定位和激光地图匹配的位姿解算方法。

背景技术

在位置服务应用中，服务的内容与模式都是与服务对象的位置密切相关的，因此对服务对象进行定位是位置服务应用首先要解决的问题。在室外环境下，目前可行的、常用的方案是采用卫星定位。但是，在室内环境或卫星定位无法正常工作的地方，必须采用其它方法来实现定位，这就是目前室内定位技术研究的内容，该研究的目标是解决定位导航的最后一公里问题，实现全地域的精确定位。

移动机器人要走向实用，必须拥有满足地形要求的运动通行能力、准确高效的定位导航能力和灵敏可靠的环境感知能力。精确的位置和姿态信息是保证移动机器人正确完成导航和控制任务的关键点之一。常用的定位方法有：里程计、惯性导航、磁罗盘、主动灯塔、卫星定位***、视觉导航、路标导航、地图模型匹配和仿生导航技术等。每种定位技术都有各自的优点和局限性，一般在实际应用中都是综合使用其中的几种，通过有效的信息融合技术，实现优缺点互补，以提高定位的精度、连续性和可靠性。

随着无线通信技术的发展和移动通信设备的广泛应用，出现了利用无线通信技术来实现定位的技术，即无线定位技术。常用的无线定位技术有：手机基站辅助定位(A-GPS)、Wifi定位、蓝牙iBeacon定位、UWB定位等。定位原理主要有四种：TOA(Time of Arrivaldata fusion，基于到达时间的数据融合)、AOA(Angle of Arrival data fusion，基于到达角度的数据融合)、RSSI(接收信号强度衰减)和信号强度指纹。移动机器人应用对定位精度要求较高，在上述无线定位技术中定位精度最高的是UWB定位技术，它理想的定位精度可以达到0.1米，在我们所做的实际测试中，证明其定位精度可以达到0.3米，这个精度还不能满足移动机器人自主导航***的要求。

得益于低成本激光雷达设备的产业化，可以利用激光测距精度高的特点，把激光扫描数据与无线定位结果进行有效融合，从而实现高精度的定位。把激光扫描数据与全局地图进行匹配，能够精确计算出偏航角，对于在平面中移动的机器人而言，这就实现了对其姿态的解算。

发明内容

针对现有技术中存在的技术问题，利用激光测距精度高的特点，把激光扫描数据与无线定位结果进行有效融合，从而实现高精度的定位。把激光扫描数据与全局地图进行匹配，能够精确计算出偏航角，对于在平面中移动的机器人而言，这就实现了对其姿态的解算，即本发明提供的这种基于无线定位和激光地图匹配的位姿解算方法，该方法既避免了无线定位精度低的缺点，又避免了激光扫描定位易受环境变化的干扰而发生定位位置跳变的情况。

为了实现上述目的，本发明采用如下技术方案。

一种基于无线定位和激光地图匹配的位姿解算方法，该方法包括如下步骤：

步骤1，把至少三个无线定位锚点部署在环境中的不同位置，以确保所有无线定位锚点不在空间形成一条直线；

步骤2，使用安装了激光扫描仪和无线定位标签的定位设备，基于SLAM同步定位与地图构建算法(Simultaneous Localization and Mapping，SLAM)构建全局激光地图，同时记录定位设备当前的位置和定位设备与各无线定位锚点之间的距离；

步骤3，利用步骤2得到的定位设备位置数据与各无线定位锚点之间的距离数据，计算无线定位锚点的位置；

步骤4，利用步骤2得到的全局激光地图，计算所有无障碍物像素点的模拟激光扫描数据；

步骤5，在对定位设备进行定位时，首先测量无线定位标签与各无线定位锚点之间的距离，再通过无线定位算法计算无线定位标签的位置；

步骤6，根据无线定位标签在定位设备上的安装位置和无线定位误差大小，以步骤5计算出的无线定位标签的位置为中心，估计出定位设备的备选位置区域；

步骤7，测量一帧激光扫描数据，在备选位置区域中的每一个无障碍物像素点处，把测得的激光扫描数据与该像素点处的模拟激光扫描数据进行匹配，得到偏航角和匹配距离；

步骤8，选出激光扫描数据匹配距离最小的像素点，其对应的位置即为定位设备当前位置，在该像素点通过激光扫描数据匹配得到的偏航角即为定位设备的偏航角。

优选的是，所述无线定位标签的位置，以回归法估计出的每个无线定位标签的位置为初始值，以计算距离与测量距离的均方误差最小为准则，利用梯度下降法，精确计算每个无线定位标签的位置。

在上述任一技术方案中优选的是，所述测量无线定位标签与各无线定位锚点之间的距离，在每一个采样时刻，测量无线定位标签到每个无线定位锚点的距离。

在上述任一技术方案中优选的是，所述每一个采样时刻的测量无线定位标签到每个无线定位锚点的距离，如果每个无线定位锚点的个数大于3个，则从中随机选择3个：如果满足平面三点定位要求，则利用三点定位法计算无线定位标签的位置；否则再次随机选择，直到计算出无线定位标签的位置。

在上述任一技术方案中优选的是，如步骤6所述的估计定位设备备选位置区域，以无线定位标签的位置为中心，以无线定位误差与无线定位标签偏离定位设备中心点的距离的和为半径，构造一个圆形的候选定位区域，在后续步骤中，激光匹配定位将在在候选定位区域中进行。

在上述任一技术方案中优选的是，如步骤4所述的计算模拟激光扫描数据，把全局地图中的候选定位点找出来，并通过仿真的方式把各定位点的模拟激光扫描数据算出来，作为地图匹配定位时的使用。

在上述任一技术方案中优选的是，根据全局地图统计候选定位点时，可以把地图中所有的无障碍物像素点作为候选定位点。

在上述任一技术方案中优选的是，该方法通过计算各偏航角情况下激光扫描数据与备选位置区域的各像素点的模拟激光数据的最小平均距离差，来确定定位设备的当前位置像素和偏航角。

本发明的基于无线定位和激光地图匹配的位姿解算方法，把无线定位方法与激光扫描定位相结合，实现了一种不依赖于卫星定位***的室内高精度定位方法，该方法同时也实现了对定位设备偏航角的计算。该方法既避免了无线定位精度低的缺点，又避免了激光扫描定位易受环境变化的干扰而发生定位位置跳变的情况。本发明实现的高精度位姿计算方法，可广泛应用于位置服务和移动机器人自主导航领域，是位置服务应用和移动机器人的关键核心技术之一。

附图说明

图1为按照本发明的基于无线定位和激光地图匹配的位姿解算方法的一优选实施例的***设备架构示意图；

图2为按照本发明的基于无线定位和激光地图匹配的位姿解算方法的一优选实施例的方法流程框图。

具体实施方式

下面结合附图和具体实施方式对本发明作详细说明，以下描述仅作为示范和解释，并不对本发明作任何形式上的限制。

基于无线定位和激光地图匹配的位姿解算方法包括如下步骤：

步骤1：把至少三个无线定位锚点部署在环境中的不同位置，确保所有无线定位锚点不在空间形成一条直线；

步骤2：使用安装了激光扫描仪和无线定位标签的定位设备，基于同步定位与地图构建算法(Simultaneous Localization and Mapping，SLAM)，构建全局激光地图，同时记录定位设备当前的位置和定位设备与各无线定位锚点之间的距离；

步骤3：利用步骤2得到的定位设备位置数据与各无线定位锚点之间的距离数据，计算无线定位锚点的位置；

步骤4：利用步骤2得到的全局激光地图，计算所有无障碍物像素点的模拟激光扫描数据；

步骤5：在对定位设备进行定位时，首先测量无线定位标签与各无线定位锚点之间的距离，再通过无线定位算法计算无线定位标签的位置；

步骤6：根据无线定位标签在定位设备上的安装位置和无线定位误差大小，以步骤5计算出的无线定位标签的位置为中心，估计出定位设备的备选位置区域；

步骤7：测量一帧激光扫描数据，在备选位置区域中的每一个无障碍物像素点处，把测得的激光扫描数据与该像素点处的模拟激光扫描数据进行匹配，得到偏航角和匹配距离；

步骤8：选出激光扫描数据匹配距离最小的像素点，其对应的位置即为定位设备当前位置，在该像素点通过激光扫描数据匹配得到的偏航角即为定位设备的偏航角。

这种基于无线定位和激光地图匹配的位姿解算方法，以回归法估计出的每个无线定位标签的位置为初始值，以计算距离与测量距离的均方误差最小为准则，利用梯度下降法，精确计算每个无线定位标签的位置。在每一个采样时刻，测量无线定位标签到每个无线定位锚点的距离。如果每个无线定位锚点的个数大于3个，则从中随机选择3个，如果满足平面三点定位要求，则利用三点定位法计算无线定位标签的位置，否则再次随机选择，直到计算出无线定位标签的位置。该方法以无线定位标签的位置为中心，以无线定位误差与无线定位标签偏离定位设备中心点的距离的和为半径，构造一个圆形的候选定位区域，在后续步骤中，激光匹配定位将在在候选定位区域中进行。该方法把全局地图中的候选定位点找出来，并通过仿真的方式把各定位点的模拟激光扫描数据算出来，然后在地图匹配定位的时候使用。根据全局地图统计候选定位点时，一般把地图中所有的无障碍物像素点作为候选定位点。该方法通过计算各偏航角情况下激光扫描数据与候选位置区域的各像素点的模拟激光数据的最小平均距离差，来确定定位设备的当前位置像素和偏航角。

具体如图1和图2所示，进行说明：

1.地图坐标系、世界坐标系、定位设备坐标系、激光扫描仪坐标系和无线定位标签坐标系：

定义地图坐标系A_m：以地图左下角的像素为原点，水平向右方向为x轴方向，垂直向上方向为y轴方向。

定义世界坐标系A_w：世界坐标系的以地图坐标系中坐标为(512,512)的像素为原点，x轴方向和y轴方向与地图坐标系相同。

定义定位设备坐标系A_v：以定位设备的自旋转轴的中心点为原点，以定位设备的前进方向为x轴方向，以定位设备向左的方向为y轴方向。

定义激光扫描仪坐标系A_L：把激光扫描仪安装在定位设备的中心，并让激光扫描仪的0角度方向与定位设备的前进方向保持一致，这样激光扫描仪坐标系A_L就与定位设备坐标系A_v相同。

定义无线定位标签坐标系A_u：以无线定位标签的射频天线中心点为原点，x轴和y轴方向与定位设备坐标系相同。

根据定义，可以计算地图坐标系A_m与世界坐标系A_w的坐标变换：T_mw和T_wm。根据定位设备的位置和偏航角，可以计算世界坐标系A_w与定位设备坐标系A_v的坐标变换：T_wv和T_vw。根据无线定位标签在定位设备上的安装位置，可以计算定位设备坐标系A_v与无线定位标签坐标系A_u的坐标变换：T_vu和T_uv。

2.使用SLAM算法构建激光全局地图：

把至少三个无线定位锚点部署在环境中的不同位置，确保所有无线定位锚点不在空间形成一条直线。

把安装了激光扫描仪和无线定位标签的定位设备固定在小车的中心，并把小车放在待构建地图的环境中的某个位置，然后启动SLAM过程。

控制小车在环境中随机移动，并遍历整个环境。

在通过SLAM方法构建激光全局地图的同时，在每个采样时刻记录定位设备在世界坐标系中的位置和偏航角，以及无线定位标签与各个无线定位锚点的距离。

3.计算无线定位锚点在世界坐标系中的位置：

根据定位设备在世界坐标系中的位置和偏航角，计算世界坐标系A_w到定位设备坐标系A_v的坐标变换：T_wv，再根据无线定位标签在定位设备坐标系A_v中的坐标，即可计算出每个采样时刻无线定位标签在世界坐标系中的位置。

利用回归法计算每个无线定位标签的位置。

以回归法估计出的每个无线定位标签的位置为初始值，以计算距离与测量距离的均方误差最小为准则，利用梯度下降法，精确计算每个无线定位标签的位置。

4.无线定位算法：

在每一个采样时刻，测量无线定位标签到每个无线定位锚点的距离。

如果每个无线定位锚点的个数大于3个，则从中随机选择3个，如果满足平面三点定位要求，则利用三点定位法计算无线定位标签的位置，否则再次随机选择，直到计算出无线定位标签的位置。

以无线定位标签的位置为中心，以无线定位误差与无线定位标签偏离定位设备中心点的距离的和为半径，构造一个圆形的候选定位区域，在后续步骤中，激光匹配定位将在在候选定位区域中进行。

定义n(n≥3)个无线定位锚点的位置：{x_a(i),y_a(i),z_a(i)},1≤i≤n。

定义无线定位标签与n个无线定位锚点的距离：{d(i)},1≤i≤n。

用回归法计算无线定位标签的位置{x,y}，(这里假设是平面定位，并且设定无线定位标签所在的高度为0，因此，实际上无线定位锚点的高度是其相对于无线定位标签的高度)：

无线定位标签与第i个无线定位锚点的距离满足：

(x-x_a(i))²+(y-y_a(i))²+z_a ²(i)＝d²(i),i＝1,2,…,n

把第i+1与第i个式子想减，得到下式

2*(x_a(i+1)-x_a(i))*x+2*(y_a(i+1)-y_a(i))*y＝(x_a ²(i+1)-x_a ²(i))

+(y_a ²(i+1)-y_a ²(i))

-(d²(i+1)-d²(i))

此时非线性方程组转换为线性形式，可以用变换后的(n-1)个样本，用线性回归法计算无线定位标签的位置{x,y}。

n只要满足n≥3条件即可实现无线定位标签的位置{x,y}的计算，n越大(即无线定位锚点的数量越多)，无线定位标签的位置{x,y}估计误差越小，定位越精准。

5.根据全局地图生成模拟的激光扫描数据：

定义激光扫描一周的激光束数：laser_scan_num，对于不能扫描整周的激光，按其扫描精度(相邻激光束的角度间隔dTheta)计算laser_scan_num＝2*pi/dTheta。

定义激光有效扫描范围：即激光扫描仪的最近有效距离和最远有效距离，超过扫描范围的数据，激光扫描仪直接返回0值表示无效数据，如果激光扫描仪有返回测距数据，并且该距离数据不在激光有效扫描范围内，则由程序强制置为0。

定义激光扫描数据：这是一个维度为laser_scan_num的距离数组，表示激光扫描仪按其扫描方向在laser_scan_num个角度上检测到的障碍物距离，无效角度或无效距离的距离数据为0。

全局地图数据定义：地图的长宽为map_sizexmap_size(像素)，每个像素代表的物理长度为pixel_size(米)，地图数据用灰度图表示，值为255的像素点表示该位置无障碍物，值为0的像素点表示是该位置是障碍物，其它值的像素点表示该位置为未探测环境。

由于小车的位置和角度的不确定性，根据激光扫描数据绘出的局部地图，与全局地图进行匹配时，既包含平移变换，又包含旋转变换，因此计算量较大。为了节省计算量，可以把全局地图中的候选定位点找出来，并通过仿真的方式把各定位点的模拟激光扫描数据算出来，然后在地图匹配定位的时候使用。

根据全局地图统计候选定位点时，一般把地图中所有的无障碍物像素点作为候选定位点。为了提高仿真数据的计算准确度，并降低候选定位点数，可以预先对地图做如下处理：把障碍物边界线做膨胀处理，把不期望作为候选定位点的像素改成不确定环境值，即改成灰度像素即可。

在计算模拟激光扫描数据时，假设全局地图是一个封闭的区域，同时不考虑激光的作用范围，那么模拟激光数据在laser_scan_num个角度上都有有效距离数据。注意，在模拟激光扫描数据中的距离值是地图坐标系下的值，与世界坐标系中的距离换算时，需要乘以pixel_size。在这里就不用换算了，因为后续的地图匹配也是在地图坐标系中进行。

6.激光扫描数据与模拟激光数据的匹配：

通过计算各偏航角情况下激光扫描数据与候选位置区域的各像素点的模拟激光数据的最小平均距离差，来确定定位设备的当前位置像素和偏航角。激光扫描数据的数学描述：

有效数据集：

I＝{i,0≤i＜laser_scan_num&laser_data[i]＞0}

无效数据集：

J＝{j,0≤j＜laser_scan_num&laser_data[j]＝0}

假定偏航角为k时的激光扫描数据与候选定位点(x，y)的模拟激光数据匹配距离：

其中：

7.在候选位置附近进行地图匹配定位算法：

通过上述无线定位方法得到了定位设备的候选位置。

在每个候选位置，计算当前激光扫描数据与该位置的模拟激光扫描数据的最佳匹配角度和匹配距离。

选择匹配距离最小的位置及其最佳匹配角度作为定位设备当前的位姿计算结果。

本发明提出的基于无线定位和激光地图匹配的位姿解算方法，不仅可以实现高精度的室内定位，为位置服务应用提供坚实的技术支撑，同时可以精确估计偏航角，因此非常适用于移动机器人自主导航领域。

以上所述仅是对本发明的优选实施方式进行描述，并非是对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通工程技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明的权利要求书确定的保护范围内。

Claims (8)

1.一种基于无线定位和激光地图匹配的位姿解算方法，其特征在于：该方法包括如下步骤：

步骤1，把至少三个无线定位锚点部署在环境中的不同位置，以确保所有无线定位锚点不在空间形成一条直线；

步骤2，使用安装了激光扫描仪和无线定位标签的定位设备，基于SLAM同步定位与地图构建算法构建全局激光地图，同时记录定位设备当前的位置和定位设备与各无线定位锚点之间的距离；

步骤3，利用步骤2得到的定位设备位置数据与各无线定位锚点之间的距离数据，计算无线定位锚点的位置；

步骤4，利用步骤2得到的全局激光地图，计算所有无障碍物像素点的模拟激光扫描数据；

步骤5，在对定位设备进行定位时，首先测量无线定位标签与各无线定位锚点之间的距离，再通过无线定位算法计算无线定位标签的位置；

步骤6，根据无线定位标签在定位设备上的安装位置和无线定位误差大小，以步骤5计算出的无线定位标签的位置为中心，估计出定位设备的备选位置区域；

步骤7，测量一帧激光扫描数据，在备选位置区域中的每一个无障碍物像素点处，把测得的激光扫描数据与该像素点处的模拟激光扫描数据进行匹配，得到偏航角和匹配距离；

步骤8，选出激光扫描数据匹配距离最小的像素点，其对应的位置即为定位设备当前位置，在该像素点通过激光扫描数据匹配得到的偏航角即为定位设备的偏航角。

2.如权利要求1所述的基于无线定位和激光地图匹配的位姿解算方法，其特征在于：计算无线定位标签的位置的方法：以回归法估计出的每个无线定位标签的位置为初始值，以计算距离与测量距离的均方误差最小为准则，利用梯度下降法，精确计算每个无线定位标签的位置。

3.如权利要求1所述的基于无线定位和激光地图匹配的位姿解算方法，其特征在于：测量无线定位标签与各无线定位锚点之间的距离的方法：在每一个采样时刻，测量无线定位标签到每个无线定位锚点的距离。

4.如权利要求3所述的基于无线定位和激光地图匹配的位姿解算方法，其特征在于：计算每一个采样时刻的测量无线定位标签到每个无线定位锚点的距离的方法：如果无线定位锚点的个数大于3个，则从中随机选择3个：如果满足平面三点定位要求，则利用三点定位法计算无线定位标签的位置；否则再次随机选择，直到计算出无线定位标签的位置。

5.如权利要求1所述的基于无线定位和激光地图匹配的位姿解算方法，其特征在于：估计定位设备备选位置区域的方法：以无线定位标签的位置为中心，以无线定位误差与无线定位标签偏离定位设备中心点的距离的和为半径，构造一个圆形的候选定位区域，在后续步骤中，激光匹配定位将在候选定位区域中进行。

6.如权利要求1所述的基于无线定位和激光地图匹配的位姿解算方法，其特征在于：计算模拟激光扫描数据的方法：把全局地图中的候选定位点找出来，并通过仿真的方式把各定位点的模拟激光扫描数据算出来，作为地图匹配定位时的使用。

7.如权利要求6所述的基于无线定位和激光地图匹配的位姿解算方法，其特征在于：根据全局地图统计候选定位点时，可以把地图中所有的无障碍物像素点作为候选定位点。

8.如权利要求1所述的基于无线定位和激光地图匹配的位姿解算方法，其特征在于：该方法通过计算各偏航角情况下激光扫描数据与备选位置区域的各像素点的模拟激光数据的最小平均距离差，来确定定位设备的当前位置像素和偏航角。