一种基于激光地图的毫米波雷达连续定位方法及装置
阅读说明：本技术 一种基于激光地图的毫米波雷达连续定位方法及装置 (Millimeter wave radar continuous positioning method and device based on laser map ) 是由 王越 尹欢 熊蓉 于 2021-09-06 设计创作，主要内容包括：本发明公开了一种基于激光地图的毫米波雷达连续定位方法及装置。包括获取机器人工作环境的先验二维激光地图,地图是预先构建的,获取移动机器人当前的二维毫米波雷达测量数据,根据神经网络提取先验激光地图和当前毫米波雷达特征等步骤。本发明在面对恶劣气候,比如雨雪天环境等,视觉和激光等传感器提供的数据会存在较大噪声,甚至无法正常工作。毫米波雷达对于光照和天气变化鲁棒。因此,本发明所提出的方法和装置适用于恶劣天气与环境下的机器人连续定位。(The invention discloses a millimeter wave radar continuous positioning method and device based on a laser map. The method comprises the steps of obtaining a prior two-dimensional laser map of a robot working environment, wherein the map is constructed in advance, obtaining current two-dimensional millimeter wave radar measurement data of the mobile robot, extracting the prior laser map and current millimeter wave radar characteristics according to a neural network and the like. When the invention is faced with severe weather, such as rain and snow environment, the data provided by sensors such as vision and laser, etc. has larger noise, even can not work normally. Millimeter wave radar is robust to illumination and weather variations. Therefore, the method and the device provided by the invention are suitable for continuous positioning of the robot in severe weather and environment.)
技术领域
本发明涉及机器人定位技术，具体地说，涉及一种基于激光地图的毫米波雷达连续定位方法及装置。
背景技术
智能移动机器人是一种能够在环境中移动的机器人，在交通、物流、农业等行业内有广泛的应用和巨大的发展潜力。移动机器人的自主定位提供了机器人在环境中的位姿，是后续导航任务的必备基础和先验条件。目前主流的移动机器人定位模式是首先构建离线地图，然后通过传感器在线定位。
在定位过程中，移动机器人的定位系统装配有相机、激光和毫米波雷达传感器。通常而言，视觉定位和激光定位可以覆盖大部分室外应用场景。然而，在恶劣天气下，比如雨天、雪天和雾天，相机个激光传感器提供的数据会存在较大噪声，甚至无法正常工作，从而对机器人的鲁棒定位造成影响。这种情况下，毫米波雷达对于光照和天气变化更为鲁棒，适用于雨雪等恶劣天气。因此，近几年毫米波雷达逐渐成为机器人定位的解决方案。
与激光传感器在激光地图上定位方法类似，一种常规的思路是首先用毫米波雷达构建地图，然后通过点云或特征匹配进行定位。但是毫米波雷达数据中存在较大噪声，并不能保证高精地图构建的完整性与一致性。此外，激光雷达地图构建方法已经成熟，如果重复用毫米波雷达进行地图构建，会造成资源与人力的浪费。考虑到激光雷达和毫米波雷达都是探距传感器，两者存在一定的共性，因此在已有的高精度激光地图上进行毫米波雷达连续定位是一个可行的方案。
发明内容
为了克服现有技术的不足，本发明的目的在于提供一种基于激光地图的毫米波雷达连续定位方法。
本发明是通过以下技术方案来实现的：
本发明公开了一种基于激光地图的毫米波雷达连续定位方法，包括以下步骤：
获取机器人工作环境的先验二维激光地图，地图是预先构建的；
获取移动机器人当前的二维毫米波雷达测量数据；
根据神经网络提取先验激光地图和当前毫米波雷达特征；
根据所提取特征和神经网络计算当前移动机器人的位姿偏移，包含平面位移和偏航角；
根据每时刻所估计的位姿偏移，用卡尔曼滤波连续估计机器人位姿。
作为进一步地改进，本发明所述的先验二维激光地图：
根据毫米波雷达传感器和激光传感器之间的标定关系，对激光点云进行高度滤波，以滤除地面等毫米波雷达传感器无法探测的范围；
根据滤波后的激光点云，通过激光SLAM技术构建三维激光点云地图；
根据构建的三维激光点云地图，通过栅格化转化为二维栅格图像用于后续的毫米波雷达定位。
作为进一步地改进，本发明所述的神经网络提取先验激光地图和毫米波雷达特征：
建立双输入的网络架构，其中包含三个U-Net架构，记作
与分别为淹没网络、毫米波雷达特征提取网络和激光特征提取网络。掩膜网络用于学习噪声的网络，其中最后的Sigmoid激活函数限制了输出掩膜的大小为[0，1]，而降噪的毫米波雷达表示通过同位像素乘法获得：其中
和Rt分别为掩模后的和原始的毫米波雷达数据，随后神经网络分别提取两类数据的特征:
其中和的最后一层为ReLU激活函数，和分别为所提取的毫米波雷达特征和激光特征。
作为进一步地改进，本发明所述的以神经网络计算当前移动机器人的位姿偏移：
给定初始位姿和真值之间的偏移量Δx应处于有限的范围内，记作-Δxm＜Δx＜Δxm，Δxm＞0，其中Δxm为范围边界；对于其他偏移量Δy和Δθ同样位于[-Δym，Δym]和[-Δθm，Δθm]之间，其中Δym和Δθm为范围边界。然后该三维范围空间以一定分辨率δx，δy，δθ划分，从而共有nxyθ＝nx×ny×nθ个所穷举的候选位姿，其中以x轴为例空间内的候选位姿记作Δxijk＝{Δxi，Δyj，Δθk}，其中1≤i≤nx，1≤j≤ny，1≤k≤nθ，而i，j，k为索引。每个全局坐标系下候选位姿将初始位姿xt通过偏移Δxijk转换计算得出，如下所示：
其中 和Δxijk分别表示转换过程、当前位姿以及偏移位姿；
在通过神经网络获取特征和后，二维特征通过以上转换过程进行相应的旋转和平移，共有个nxyθ个转后的特征同时特征保持不变并复制相同数量，然后元素相减特征计算得到特征差值ΔE，考虑到雷达数据存在遮挡问题，进一步将ΔE划分为k×k个小片段，对于每一个小片段补丁，建立补丁网络计算每个补丁的差异度，最终，平均k2个补丁差异作为每个毫米波雷达特征和激光地图特征之间的相似度，该平均后的差异向量的长度为nxyθ。
当某个特征差异值更小时，候选位姿更接近真实位姿采用最小归一化指数模块(Softmin)归一化差异向量，形成概率分布；将该概率分布重塑为三维代价体素表示V(Δxijk)；根据当前毫米波雷达观测与已有激光地图，所提出的网络生成了偏移位姿的概率分布，如下式：
其中为神经网络，V为代价体素；
在每个轴上累积概率以计算边缘分布，如下：
其中概率Px(Δxi)，Py(Δyi)和Pθ(Δθk)由体素
V中各轴计算得出；
从而可以得出偏移的估计值，包含位移和旋转，如下：
作为进一步地改进，本发明所述的卡尔曼滤波连续估计机器人位姿：
卡尔曼滤波状态估计包含两部分，运动模型组成的预测步骤和观测模型组成的更新步骤，构成高斯噪声下的贝叶斯递归估计。
作为进一步地改进，本发明所述的运动模型组成的预测步骤：使用ICP匹配或者车轮里程作为运动模型，用于估计相邻两帧毫米波雷达之间的里程，即从Rt-1到Rt的ut-1，其中Rt-1和Rt分别为前后时刻的毫米波雷达数据，ut-1为时刻之间的运动量。基于连续的里程计，预测步骤如下所示：
其中f(·)表示累积运动至前一帧位姿，而Ft是f(·)的雅各比，预测位姿由此式计算得出，协方差也由里程协方差∑m更新，后者是预先设定的系数。
作为进一步地改进，本发明所述的观测模型组成的更新步骤：使用网络作为观测模型，在预测位姿上，网络输出估计的偏移位姿Δxijk，然后生成全局观测其中为式中的位姿转换操作。根据输出的概率Px，Py与Pθ计算相应的观测协方差∑o，卡尔曼滤波的更新步骤如下所示：
其中，K，和分别代表卡尔曼增益，所估计位姿以及协方差矩阵。
作为进一步地改进，本发明所述的方法通过以下模块执行：
第一获取模块，用于获取当前场景的先验二维地图，先验二维地图是事先构建的；
第二获取模块，获取移动机器人当前的二维毫米波雷达测量数据；
第一计算模块，根据所提取特征和神经网络计算当前移动机器人的位姿偏移，包含平面位移和偏航角；
第二计算模块，用卡尔曼滤波连续估计机器人位姿。
第一获取模块和第二获取模块的信息作为第一计算模块的输入，第一计算模块的计算结果作为第二计算模块的输入，信息流向均为单向。
本发明还公开了一种基于激光地图的毫米波雷达连续定位装置，包括：毫米波雷达传感器、处理器、存储器以及存储在存储器中的程序；
毫米波雷达传感器用于获取机器人当前毫米波雷达数据；
处理器执行计算机存储器中的程序实现毫米波雷达连续定位方法的步骤；
该方法运行时，顺序为所述的毫米波雷达传感器获取的信息先通过有线连接传输到处理器，然后处理器执行存储器中的程序实现定位算法，最后相关处理结果有线传输存储到存储器中。
本发明的有益效果如下：
1.面对恶劣气候，比如雨雪天环境等，视觉和激光等传感器提供的数据会存在较大噪声，甚至无法正常工作。毫米波雷达对于光照和天气变化鲁棒。因此，本发明所提出的方法和装置适用于恶劣天气与环境下的机器人连续定位。
2.卡尔曼滤波是可微分的，本发明的后端的卡尔曼滤波估计也可以作为一种回归网络参与训练，这意味着前端的观测模型，即神经网络可以由反向传播的梯度更新并训练。所有模块可以端到端训练。
3.本发明的位移漂移为1.09％/100米，朝向漂移为0.0037度米。经验证本发明完整地在90公里的真实路段上运行。本发明的定位精度优于里程计以及目前的毫米波雷达SLAM等方法。
附图说明
图1为本发明基于激光地图的毫米波雷达连续定位的流程示意图；
具体实施方式
：
本发明公开了一种基于激光地图的毫米波雷达连续定位方法及装置。方法包括：1)获取机器人工作环境的先验二维激光地图，地图是预先构建的；先验二维激光地图：
根据毫米波雷达传感器和激光传感器之间的标定关系，对激光点云进行高度滤波，以滤除地面等毫米波雷达传感器无法探测的范围；
根据滤波后的激光点云，通过激光SLAM等技术构建三维激光点云地图；
根据构建的维激光点云地图，通过栅格化转化为二维栅格图像用于后续的毫米波雷达定位。
2)获取移动机器人当前的二维毫米波雷达测量数据；
3)根据神经网络提取先验激光地图和当前毫米波雷达的特征：
建立双输入的网络架构，其中包含三个U-Net架构，记作
与分别为淹没网络、毫米波雷达特征提取网络和激光特征提取网络。掩膜网络用于学习噪声的网络，其中最后的Sigmoid激活函数限制了输出掩膜的大小为[0，1]，而降噪的毫米波雷达表示通过同位像素乘法获得：其中
和Rt分别为掩模后的和原始的毫米波雷达数据。随后神经网络分别提取两类数据的特征:
其中和的最后一层为ReLU激活函数，和分别为所提取的毫米波雷达特征和激光特征。
4)根据所提取特征和神经网络计算当前移动机器人的位姿偏移，包含平面位移和偏航角；以神经网络计算当前移动机器人的位姿偏移：给定初始位姿和真值之间的偏移位姿应处于有限的范围内，记作-Δxm＜Δx＜Δxm，Δxm＞0，其中Δxm为范围边界；对于Δy和Δθ同样位于[-Δym，Δym]和[-Δθm，Δθm]，其中Δym和Δθm为范围边界。然后该三维范围空间以一定分辨率δx，δy，δθ划分，从而共有nxyθ＝nx×ny×nθ个所穷举的候选位姿，例如空间内的候选位姿记作
Δxijk＝{Δxi，Δyj，Δθk}，其中1≤i≤nx，1≤j≤ny，1≤k≤nθ。因此，每个全局坐标系下候选位姿可以将初始位姿xt通过偏移Δxijk转换计算得出，如下所示：
其中 和Δxijk分别表示转换过程,当前位姿以及偏移位姿。
在通过神经网络获取特征和后，二维特征通过以上转换过程进行相应的旋转和平移，共有个nxyθ个转后的特征，同时特征保持不变并复制相同数量。然后元素相减特征计算得到特征差值ΔE。考虑到雷达数据存在遮挡问题，进一步将ΔE划分为k×k个小片段。对于每一个小片段补丁，建立补丁网络计算每个补丁的差异度。最终，本发明平均k2个补丁差异作为每个毫米波雷达特征和激光地图特征之间的相似度，该平均后的差异向量的长度为nxtθ。
当某个特征差异值更小时，候选位姿更接近真实位姿采用最小归一化指数模块(Softmin)归一化差异向量，形成概率分布；将该概率分布重塑为三维代价体素表示V(Δxijk)；根据当前毫米波雷达观测与已有激光地图，所提出的网络生成了偏移位姿的概率分布，如下式所述：
其中为神经网络，V为代价体素。
然后，本发明在每个轴上累积概率以计算边缘分布，如下：
其中概率Px(Δxi)，Py(Δyj)和Pθ(Δθk)由体素
V中各轴计算得出。
从而可以得出偏移的估计值，包含位移和旋转，如下：
5)根据每时刻所估计的位姿偏移，用卡尔曼滤波连续估计机器人位姿，卡尔曼滤波连续估计机器人位姿：
卡尔曼滤波状态估计包含两部分，首先是运动模型组成的预测步骤，然后是观测模型组成的更新步骤，从而构成高斯噪声下的贝叶斯递归估计。
使用ICP匹配或者车轮里程作为运动模型，用于估计相邻两帧毫米波雷达之间的里程，即从Rt-1到Rt的ut-1，其中
Rt-1和Rt分别为前后时刻的毫米波雷达数据，ut-1为时刻之间的运动量。基于连续的里程计，预测步骤如下所示：
其中f(·)表示累积运动至前一帧位姿，而Ft是f(·)的雅各比。预测位姿由此式计算得出。协方差也由里程协方差∑m更新，后者是预先设定的系数。
在更新阶段，本发明使用网络作为观测模型。在预测位姿上，网络输出估计的偏移位姿Δxijk，然后生成全局观测其中为式中的位姿转换操作。类似于GPS信号，可以根据zt推出观测模型。此外，本发明也根据输出的概率Px，Py与Pθ计算相应的观测协方差∑o。总之，卡尔曼滤波的更新步骤如下所示：
其中，K，和分别代表卡尔曼增益，所估计位姿以及协方差矩阵。
神经网络训练包含以下方式与策略：
根据边缘分布，本发明首先设计了两种损失函数用于训练网络
首先，本发明的估计问题可以作为三个{Δxi，Δyj，Δθk}内的独立的分类任务，因此可以使用交叉熵损失函数训练，如下：
其中Cx，Cy和Cθ为真值的独热编码(one-hot encoding)。
另一方面，作为回归问题，可以使用三个边际分布构建期望值。所估计的偏移位姿为然后第二个损失函数通过计算真值与之间的平方和得出，如下所示：
其中α为一个常量，用于平衡位移度量(m)和角度度量(o)。
此外，连续定位的卡尔曼滤波中所有模块是可微分的，因此卡尔曼滤波也可以作为一种回归网络，这意味着观测模型可以由反向传播的梯度更新并训练，因此，本发明也设计了损失函数用于训练前端特征提取的神经网络，以及位姿回归的神经网络。
从概率的角度出发，卡尔曼滤波估计器生成了连续的高斯分布下的三维位姿，即所以，根据已有的连续的真值位姿
本发明通过最大后验概率训练网络参数，如下：
其中det(·)为矩阵行列式，然后通过取负对数操作使其成为最小化：
基于以上分析，通过舍弃常数项，本发明可以简化地推出第三个损失函数可以用于端到端地训练连续的卡尔曼滤波定位：
其中β为用于平衡的因子，而k表示连续定位的帧数。该损失函数构成了一种最大似然和最小化不确定性之间的平衡。
下面，结合说明书附图以及具体实施方式，对本发明的技术方案作进一步地描述：
图1为本发明基于激光地图的毫米波雷达连续定位的流程示意图。本发明公开了一种基于激光地图的毫米波雷达连续定位的方法，分为以下几个步骤：
1.获取机器人工作环境的先验二维激光地图。可以按如下步骤获取：通过激光SLAM技术构建三维激光地图；去除地面等毫米波雷达无法探测的点云部分；转换滤波后的三维地图至二维栅格地图。此外，为了保证后续计算的一致性，每一次计算激光地图的大小通过裁剪，保持和毫米波雷达一样的尺寸，裁剪中心即为当前机器人的定位位姿。
2.获取机器人运行时的二维毫米波雷达数据，本发明方法适用于多个毫米波雷达传感器或者单个360度探距范围的毫米波雷达传感器。
3.通过U-Net神经网络分别提取激光地图和毫米波雷达的特征。具体而言，神经网络提取激光地图的二维特征其最后一层为ReLU激活函数；对于毫米波雷达数据，首先通过掩膜网络
生成掩模，然后同为像素相乘后，再通过神经网络提取毫米波雷达特征其中最后一层为Sigmoid激活函数，最后一层为ReLU函数。
4.在获得的特征基础上，通过一种穷举的策略使用神经网络计算出当前机器人的位姿偏移，该偏移是指预估计位姿与真实位姿真值位姿之间的偏移。具体而言，分为以下几个步骤：
在有限的二维空间内穷，以一定的分辨率在偏移和旋转轴上穷举候选位姿，共有nxyθ＝nx×ny×nθ个所穷举的候选位姿。空间内的候选位姿记作Δxijk＝{Δxi，Δyj，Δθk}，其中1≤i≤nx，1≤j≤ny，1≤k≤nθ。
同时，旋转和平移激光地图特征，即生成nxyθ个候选特征并复制相同数量的毫米波雷达特征。本质上，对于一个候选位姿，如果其接近于真实位姿 应该与更相似。然后元素相减特征计算得到特征差值ΔE，进一步将ΔE划分为k×k个小片段。对于每一个小片段补丁，建立补丁网络计算每个补丁的相似度。最终，平均k2个补丁相似度，并通过Softmin进行归一化，从而得到每个候选位姿的概率。在此基础上，计算得出偏移的位移和旋转。
5.根据所估计的偏移，本发明通过卡尔曼滤波连续定位机器人。具体而言，在更新阶段，所估计的偏移为滤波定位提供了观测信息，而概率分布的方差可以作为滤波定位的协方差代入计算。在预测阶段，可以使用里程计作为定位的估计。
本发明可用于众多通用或专用的计算系统环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等。
本发明公开了基于激光地图的毫米波雷达连续定位方法，上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。