CN109275093B

CN109275093B - 基于uwb定位与激光地图匹配的定位方法和移动终端

Info

Publication number: CN109275093B
Application number: CN201811169738.5A
Authority: CN
Inventors: 高子庆
Original assignee: Shenzhen Reeman Intelligent Equipment Co ltd
Current assignee: SHENZHEN REEMAN INTELLIGENT EQUIPMENT Co.,Ltd.
Priority date: 2018-10-08
Filing date: 2018-10-08
Publication date: 2021-03-09
Anticipated expiration: 2038-10-08
Also published as: CN109275093A

Abstract

本发明提供一种基于UWB定位与激光地图匹配的定位方法，包括：获取当前状态下移动终端第一位置信息，第一位置信息为通过位于移动终端上的UWB标签与设置在四周的UWB基站而确定的位置坐标；根据第一位置信息获取预设特征数据，预设特征数据为移动终端在预设条件下选取的在预设激光地图中表征当前位置的第一激光数据；获取目标特征数据，目标特征数据为移动终端上的激光雷达测量的表征当前目标位姿的第二激光数据；将目标特征数据与预设特征数据进行匹配以得到移动终端的精确位姿。本发明采用激光测距方式进行定位，不受外界光线的影响；通过uwb锁定目标区域，再在小范围内用激光特征匹配精确定位，定位精度、速度、稳定性与成本低。

Description

基于UWB定位与激光地图匹配的定位方法和移动终端

技术领域

本发明涉及车辆安全领域，具体而言，本发明涉及一种基于UWB定位与图像特征匹配的导航方法和装置。

背景技术

移动机器人在导航过程中，定位和导航都很重要。如果定位偏差大，会导致机器人导航出现乱走甚至导航失败的情况，所以导航对定位的依赖非常大，而且对定位精度的要求也比较高。同时，导航算法的质量也直接关系到机器人行走的步态

目前室外的定位技术主要是卫星定位(如：gps)，和手机基站定位。但是在室内卫星信号太弱，而且精度不够，基本不能定位，手机基站定位误差非常大。所以需要在信号不好的地方和室内的时候，就需要采用其他的定位方式。

目前的室内定位技术主要有：码盘，惯性传感器，Wi-Fi定位，蓝牙定位，uwb无线定位，匹配定位。他们各有优缺点，一般都需要结合多种方法才能完成精准的定位。

1.码盘、惯性传感器：这种定位方法精度较高，但有累积误差，随着行驶时间、距离的增加，误差也不断增大。此外，这两者都是计算相对位姿，无法直接获取绝对位姿。因此这两者适合在知道精确的绝对位姿后持续获取精确的位姿，当进行长时间、长距离的精确定位时，需要经常校正。

2.Wi-Fi定位、蓝牙定位、uwb无线定位：能够获取绝对位置，但是没有姿态信息，精度较差且不稳定，都会受障碍物和电磁环境的影响，比如穿墙、穿过金属物体后或周围电磁环境复杂时精度变差。但是其中精度最高，抗干扰最强的定位方式就是uwb无线定位。Uwb理论定位误差可以在10cm以内，在实际的使用虽然存在干扰，但误差一般都在0.5m以内，满足粗略定位的需求。

3.匹配定位：包括激光匹配，图像匹配。预先保存地图，利用地图中的特征点来进行匹配定位，能够获取精确的位姿，但运算量较大，很难做到实时获取位姿，并且在遇到相似的场景时会得到多个匹配结果。

以上方案各有优劣，但目前很少有能够取长补短、同时兼顾性价比的导航定位方案，这一领域的市场需求并未得到满足。

发明内容

本发明的目的旨在至少能解决上述的技术缺陷之一，公开一种能够精准定位的方法和装置。

一方面本发明公开一种基于UWB定位与激光地图匹配的定位方法，包括：

获取当前状态下移动终端第一位置信息，所述第一位置信息为通过位于移动终端上的UWB标签与设置在四周的UWB基站而确定的位置坐标；

根据所述第一位置信息获取预设特征数据，所述预设特征数据为所述移动终端在预设条件下选取的在预设激光地图中表征当前位置的第一激光数据；

获取目标特征数据，所述目标特征数据为所述移动终端上的激光雷达测量的表征当前目标位姿的第二激光数据；

将所述目标特征数据与预设特征数据进行匹配以得到所述移动终端的精确位姿。

可选的，所述第一位置信息的获取还包括通过高斯牛顿法进行位置误差消除，所述位置误差采用最小二乘法来表示，其公式为：

其中，x，y为预测的坐标，为x_k，y_k为第k个基站的坐标，d_k为测量到的距离。

可选的，所述预设条件为以第一位置信息为圆心以预设长度为半径做圆。

可选的，所述目标特征数据与预设特征数据进行匹配以得到所述移动终端的精确位姿的方法包括：

通过旋转矩阵和位移矢量匹配预设特征数据与目标特征数据，得出相对位姿变化值；

相对位姿变化值与多个所述预设特征数据相加得到多个第二位置信息；

将多个所述第二位置信息与所述第一位置信息对比，选取与第一位置信息最接近的第二位置信息作为精确位姿。

可选的，所述预设激光地图的获取方法包括：

获取所述移动终端在远程控制设备的控制下移动时，所述激光雷达测量的所述移动终端与周围障碍物在各个方向的第一激光数据；

获取安装在所述移动终端上的码盘在所述移动终端移动时的相对位移；

通过所述第一激光数据与所述相对位移基于slam算法构建成预设激光地图。

可选的，所述激光地图中的第一激光数据为根据所述移动终端上的激光雷达每隔一段距离或角度测量得到的移动终端与各个障碍物之间的距离分布曲线；

所述第二激光数据为当前移动状态下表征当前各个方向移动终端与各个障碍物之间的距离分布曲线。

可选的，根据所述曲线提取出特征点，并将特征点以特征描述子的方式保持，所述特征描述子的计算公式为：

hdet(j)＝#{pi≠pdet：(pi-pdet)∈binj}

其中，binj通过离散点的距离和视角来定义。

可选的，所述第一激光数据与所述第二激光数据的匹配方法包括：

定义积分算子，将输入的所述第二激光数据的特征描述子映射到多个尺度上；

通过用采样密度对高斯核进行归一化，使算子独立于曲线的采样密度；

通过使用高斯核在局部进行核密度估计来估算尺度中的采样密度，在每个尺度产生一条相应的平滑曲线；

通过指数衰减表达式，匹配与所述第二激光数据在预设条件下的第一激光数据。

可选的，所述第一激光数据与所述第二激光数据的匹配还包括特征点匹配，所述特征点匹配的方法包括：

设定一个cost的阈值；

采用卡方检验统计量计算每两个特征点的cost值；

当cost值小于阈值的时候，认定这两个特征点匹配成功。

另一方面，本申请公开一种移动终端，包括激光雷达、码盘和UWB定位装置和处理器，所述处理器配置上述任意一项所述的基于UWB定位与激光地图匹配的定位方法，所述激光雷达、码盘和UWB定位装置分别与处理器电连接通信。。

本发明的有益效果为：

1)采用激光测距方式进行定位，不受外界光线的影响，可以在夜间进行准确定位；

2)本方案通过uwb快速锁定目标区域，再在小范围内用激光特征匹配精确定位，同时满足了定位的实时性与准确性，对传感器性能无特殊要求，有效改善了现有定位方案的精度、速度、稳定性与成本无法兼顾的现状；

3)搭建预设地图，并在实际移动中与预设地图做比较，达到三维空间避障的目的。

本发明附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为一个实施例的基于UWB定位与激光地图匹配的定位方法流程图；

图2为一个实施例的述预设激光地图的获取方法流程图；

图3为一个实施例中描述子获取示意图；

图4为一个实施例中得到精确位姿的方法流程图；

图5为本实施例特征点匹配的方法流程图；

图6为本发明一实施例中的移动终端结构示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样被特定定义，否则不会用理想化或过于正式的含义来解释。

本技术领域技术人员可以理解，这里所使用的远程控制设备，其包括但不限于手机、计算机、网络主机、单个网络服务器、多个网络服务器集或多个服务器构成的云。在此，云由基于云计算(Cloud Computing)的大量计算机或网络服务器构成，其中，云计算是分布式计算的一种，由一群松散耦合的计算机集组成的一个超级虚拟计算机。本发明的实施例中，远程控制设备、终端设备与WNS服务器之间可通过任何通信方式实现通信，包括但不限于，基于3GPP、LTE、WIMAX的移动通信、基于TCP/IP、UDP协议的计算机网络通信以及基于蓝牙、红外传输标准的近距无线传输方式。

本发明公开一种基于UWB定位与激光地图匹配的定位方法，请参阅图1，包括：

S100、获取当前状态下移动终端第一位置信息，所述第一位置信息为通过位于移动终端上的UWB标签与设置在四周的UWB基站而确定的位置坐标；

UWB(Ultra Wideband)是一种无载波通信技术，利用纳秒至微秒级的非正弦波窄脉冲传输数据，常被用于室内短距离定位。本申请中在移动终端上安装设置有UWB标签，在移动终端所移动的空间内设置有多个UWB基站，当UWB标签以广播的形式按照一定的频率发送脉冲信号，UWB基站接收相关信号，并进行回复，相关的处理装置将发消息的时间间隔的1/2和电磁波的传输速度相乘可以得到UWB标签与UWB基站的距离。本申请中，设置有四个UWB基站，四个UWB基站分别呈正方形方式排列，以便于准确进行定位。

由于上述多个UWB基站是按照四边形的方式排列的，故可以以四边形的中的某个位置作为原点，从而得到移动终端的具体定位。该原点可以位于其中一个UWB基站上，还可以是四边形的中心位置，或者是任何一条相交线。

确定了原点位置，当移动终端移动时，则可通过四个UWB基站获取对一个的位置坐标，即当前状态下的第一位置信息。

在本申请中，通过多个UWB基站获取的第一位置信息可能存在定位误差，因此本申请中通过高斯牛顿法进行位置误差消除，所述位置误差采用最小二乘法来表示，其公式为：

其中，x，y为预测的坐标，为x_k，y_k为第k个基站的坐标，d_k为测量到的距离。此时，误差公式是非线性的，因此我们用高斯牛顿法迭代逼近其极小值：

具体计算公式为，将e(x)一阶展开：

e(x+Δx)≈e(x)+J(x)Δx

其中，J(x)为e(x)关于x的导数。此时，求使e(x)最小的x的问题就变成了求解合适的变量值△x使得函数||e(x+△x)||的值达到最小：

此处的变量是Δx，因此是线性方程。将上式中的平方项展开：

求上式关于Δx的导数，并令其等于零：

2J(x)^Te(x)+2J(x)^TJ(x)Δx＝0

可得到如下方程组：

J(x)^TJ(x)Δx＝-J(x)^Te(x)

称为高斯牛顿方程。把左边的系数定义为H，右边定义为g，上式变为：

HΔx_k＝g

具体的执行步骤：

给定一个初始坐标x0,y0；

对于第k次迭代，求出当前的雅克比矩阵J(x_k)和误差e(x_k)；

求解增量方程NΔx_k＝g；

若Δx_k足够小，则停止，此时的x即为一个符合要求的解；

否则，令x_k+1＝x_k+Δx_k，继续迭代。

通过上述计算方式，得出基于UWB定位的精确坐标。

S200、根据所述第一位置信息获取预设特征数据，所述预设特征数据为所述移动终端在预设条件下选取的在预设激光地图中表征当前位置的第一激光数据；

所述预设条件为以第一位置信息为圆心以预设长度为半径做圆。在一实施例中，所述预设长度为2米，当通过步骤S100获得了精确的第一位置信息后，即可以该精确的坐标位置为圆心，以2米为半径做圆，得到一个范围区域，在预设激光地图中，获取该范围区域内的所有激光数据作为预设特征数据，由于所述激光数据是通过移动终端在建图时每隔一段时间段或者每隔一段距离获取的数据，故在上述范围区域内可以获得多个预设特征数据。该预设特征数据不仅包括前后左右多个不同距离上的激光数据，还包括在同一距离的位置上，不同方位的激光数据。

进一步的，在一实施例中，所述预设特征数据为所述移动终端在预设条件下选取的在预设激光地图中表征当前位置的第一激光数据；请参阅图2，所述预设激光地图的获取方法包括：

S210、获取所述移动终端在远程控制设备的控制下移动时，所述激光雷达测量的所述移动终端与周围障碍物在各个方向的第一激光数据；

第一激光数据根据移动终端移动的位置以及所旋转的方向进行一一匹配和记录。由于移动终端所处的周围环境可能有多个不同的障碍物，因此，对于不同距离和不同方向上的第一激光数据是不一样的。

本申请中，所述第一激光数据为根据所述移动终端上的激光雷达每隔一段距离或角度测量得到的移动终端与障碍物之间的距离分布曲线。

进一步的通过不同距离和不同角度的表征移动终端与障碍物的曲线，可从中提取出特征点，具体的，其特征点的提取方法包括：

定义积分算子，将输入曲线映射到多个尺度上：

S(α(s)；t)＝∫_Γk(s，u；t)α(u)du

k(s，u；t)＝N((s-ut)

其中，Γ是输入曲线，α是曲线在测地线坐标s中的参数，k为一个高斯核。然后通过用采样密度p(s；t)对高斯核进行归一化，使算子独立于曲线的采样密度：

p(s；t)＝∫k(s，u；t)p(u)du

通过使用高斯核在局部进行核密度估计来估算尺度t中的采样密度，会在每个尺度产生一条相应的平滑曲线：

通过下面的指数衰减表达式，可以找到我们需要的特征点：

其中，

是原曲线与滤波后的曲线在笛卡尔空间中的误差，在尺度t中的特征点对应于t等于平滑曲线

的局部曲率的逆的位置。

本特征检测算子分别检测在最大尺度上的距离不连续的背景部分、在所有尺度上的前景部分、所有尺度上响应的凸角和第一、第二尺度上响应的凹钝角。

进一步的，本申请中，采用局部形状上下文来描述特征点。选择线性极坐标直方图，因为距离数据中的噪声类型和范围与图像数据中的噪声不同。距离数据的噪音通常在径向上，并且幅度较大。因此，相对于局部结构，噪声更容易被捕获到，而将其放到极坐标中后，这种效果会减弱。通常情况下，上述提取的特征点P的描述子可表达为：

h_det(j)＝#{p_i≠p_det：(p_i-p_det)∈bin_j}

其中，bin_j通过离散点的距离和视角来定义。

请参阅图3，以每一个特征点作为中心点画一组同心圆，我们设同心圆有5层，并按角度分成12个分区，这样就存在5*12＝60个网格，接着统计其他点在这60个网格中的分布情况。这样就可以用一个长度为60的数组表示出其他点在当前点的周围的分布情况，以此作为该特征点的描述子。

S220、获取安装在所述移动终端上的码盘在所述移动终端移动时的相对位移；

码盘通常安装在移动终端的底部，靠近滑轮的位置，以对移动终端的移动以及旋转进行控制。通过对移动终端设定一个初始位置，当移动终端移动或者旋转的时候，则可通过码盘获取移动终端移动时的相对位移，该相对位移需要与激光雷达测量的激光数据一一对应，以便于后续的匹配和定位

S230、通过所述第一激光数据与所述相对位移基于slam算法构建成预设激光地图。

SLAM(simultaneous localization and mapping),也称为CML(ConcurrentMapping and Localization),即时定位与地图构建，或并发建图与定位。

通过将上述的第一激光数据与码盘获取的相对位移一一匹配即可根据slam算法构建成预设激光地图。

S300、获取目标特征数据，所述目标特征数据为所述移动终端上的激光雷达测量的表征当前目标位姿的第二激光数据；

在获取第一位置信息的同时，通过激光雷达可测量到所述移动终端在当前位置的第二激光数据，该激光数据表示通过激光雷达测量得到的当前移动终端的位姿态。

第一激光数据也是由移动终端上的激光雷达获得的，只是获取的时机不一样，因此所述第二激光数据为当前移动状态下表征当前各个方向移动终端与各个障碍物之间的距离分布曲线，同样可根据上述方式计算得到曲线中特征点的描述子。

S400、将所述目标特征数据与预设特征数据进行匹配以得到所述移动终端的精确位姿。

当获取了目标特征数据和多个预设特征数据后，则可将目标特征数据和预设特征数据进行一一对比。由于预设特征数据不仅包括不同距离上的激光数据，还包括同一距离，不同方向的激光数据，而目标特征数据为某一固定距离以及固定方向的激光数据，因此通过将目标特征数据与预设特征数据进行匹配，则可从预设特征数据中匹配出最为接近的预设特征数据。由于第一激光数据的获得是有距离差或时间差的，因此获得的最为接近的预设特征数据可能不止一个，这需要从中进一步进行匹配。

在本申请中，请参阅图4，所述目标特征数据与预设特征数据进行匹配以得到所述移动终端的精确位姿的方法包括：

S410、通过旋转矩阵和位移矢量匹配预设特征数据与目标特征数据，得出相对位姿变化值；

在步骤S200中，通过UWB标签获得了坐标位置，以2米为半径作圆得到一个范围区域，在该范围区域内匹配得到最接近的第一位置信息的预设特征数据，由于激光雷达是通过一定的距离、方向或时间间隔测量得到的，故当前第一位置信息不一定完全能与第一激光数据匹配，可能存在多个较为接近的第一激光数据，做为预设特征数据。获取这些预设特征数据的方法可以是选取一个阈值，当匹配的数据在阈值范围内，则可确定该第一激光数据为预设特征数据。

在一实施例中，对于任一对匹配的预设特征数据的激光点集F1,F2，假设其对应的特征点集为P，Q：

P＝{P1,P2,…,Pn}∈F1

Q＝{Q1,Q2,…,Qn}∈F2

通过一个旋转矩阵R和位移矢量t，将两帧图像对应起来：

而由于误差的存在，上式等号不可能成立，所以采用最小化误差的方法来求解：

根据上式使用PnP方法求得最优的R和t，即可算出预设特征数据对应的相对位姿变化值。

S420、相对位姿变化值与所述预设特征数据相加得到多个第二位置信息；

由于对上述每一个预设特征数据计算其相对位姿变化值，将所述相对位姿各自与所述预设特征数据相加，即可得到多个第二位置信息，所述第二位置信息为计算出来的移动终端的坐标位置信息以及旋转的角度。

S430将多个所述第二位置信息与所述第一位置信息对比，选取与第一位置信息最接近的第二位置信息作为精确位姿。

在本申请中，第二位置信息与第一位置信息对比，实际上是预设特征数据与目标特征数据的特征匹配，请参阅图5，所述特征点匹配的方法包括：

S431、设定一个cost的阈值；

S432、采用卡方检验统计量计算每两个特征点的cost值；

其中k指的是第k个网格。g和h分别表示两个点集中的某个特征点。

S433、当cost值小于阈值的时候，认定这两个特征点匹配成功。

当上述特征点匹配成功，则计算该第二位置信息与第一位置信息之间的距离，选取最为靠近第一位置信息的第二位置信息作为移动终端的精确位姿。

另一方面，请参阅图6，本申请公开一种移动终端100，包括激光雷达110、码盘120和UWB定位装置130和处理器(图未示)，所述处理器配置上述任意一项所述的基于UWB定位与激光地图匹配的定位方法，所述激光雷达、码盘和UWB定位装置分别与处理器电连接通信。

优选的一种方案为，移动终端100为一种可移动的机器人，所述码盘120安装在可移动机器人的下端，与移动终端的滑轮连接，激光雷达110安装在可移动机器人的身体正面，通过激光雷达110读取移动终端100与障碍物之间的距离，UWB定位装置130安装在移动终端100上。

另一方面，本发明公开一种非临时性计算机可读存储介质，当所述存储介质中的指令由移动终端的处理器执行时，使得移动终端能够执行一种基于基于UWB定位与激光地图匹配的定位方法，所述方法包括上述任意一项所述的基于UWB定位与激光地图匹配的定位方法。

应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims

1.一种基于UWB定位与激光地图匹配的定位方法，其特征在于，包括：

将所述目标特征数据与预设特征数据进行匹配以得到所述移动终端的精确位姿；

所述激光地图中的第一激光数据为根据所述移动终端上的激光雷达每隔一段距离或角度测量得到的移动终端与各个障碍物之间的距离分布曲线；

所述第二激光数据为当前移动状态下表征当前各个方向移动终端与各个障碍物之间的距离分布曲线；

通过不同距离和不同角度的表征移动终端与障碍物的曲线，从中提取出特征点，其特征点的提取方法包括：

定义积分算子，将输入曲线映射到多个尺度上；

通过使用高斯核在局部进行核密度估计来估算每个尺度中的采样密度，会在每个尺度产生一条相应的平滑曲线；

通过指数衰减表达式，找到需要的特征点，在每个尺度中的特征点等于对应于每个尺度中平滑曲线的局部曲率的位置。

2.根据权利要求1所述的基于UWB定位与激光地图匹配的定位方法，其特征在于，所述第一位置信息的获取还包括通过高斯牛顿法进行位置误差消除，所述位置误差采用最小二乘法来表示，其公式为：

3.根据权利要求1所述的基于UWB定位与激光地图匹配的定位方法，其特征在于，所述预设条件为以第一位置信息为圆心以预设长度为半径做圆。

4.根据权利要求1-3任意一项所述的基于UWB定位与激光地图匹配的定位方法，其特征在于，所述目标特征数据与预设特征数据进行匹配以得到所述移动终端的精确位姿的方法包括：

5.根据权利要求1所述的基于UWB定位与激光地图匹配的定位方法，其特征在于，所述预设激光地图的获取方法包括：

6.根据权利要求1所述的基于UWB定位与激光地图匹配的定位方法，其特征在于，根据所述曲线提取出特征点，所述第一激光数据与所述第二激光数据的匹配还包括特征点匹配，所述特征点匹配的方法包括：

设定一个cost的阈值；

采用卡方检验统计量计算每两个特征点的cost值；

当cost值小于阈值的时候，认定这两个特征点匹配成功。

7.一种移动终端，其特征在于，包括激光雷达、码盘和UWB定位装置和处理器，所述处理器配置上述权利要求1-6任意一项所述的基于UWB定位与激光地图匹配的定位方法，所述激光雷达、码盘和UWB定位装置分别与处理器电连接通信。