一种无导航卫星信号下的自主定位方法技术

本发明专利技术涉及一种无导航卫星信号下的自主定位方法，属于无人车、机器人技术领域。本发明专利技术采用正态分布变换同步定位与地图生成(NDT‑SLAM)的自主定位方法，通过正态分布变换占用地图(NDT‑OM)方法形成基于三维激光雷达扫描数据的NDT‑OM局部地图，并通过NDT‑D2D匹配方法完成局部NDT‑OM地图之间的匹配，并在此基础上融合由惯性导航系统(INS)与里程计生成的位置估计信息与局部地图匹配信息，该方法能够有效融合激光雷达、INS与里程计等多源传感器信息，实现无人车、机器人等智能系统在无导航卫星信号下的准确自主定位。

【技术实现步骤摘要】
一种无导航卫星信号下的自主定位方法
本专利技术属于无人车、机器人
，具体涉及一种无导航卫星信号下的自主定位方法。
技术介绍
无人车、机器人等智能系统自主行驶与作业时，只有获取准确的自主定位信息才能进行准确的环境感知、自主导航与自主作业。真实复杂环境中，当导航卫星系统受到电磁干扰、物体遮蔽等环境影响时，无人车、机器人等智能系统可能完全无法获取导航卫星信号。因此，如何实现智能系统在无导航卫星信号下的准确自主定位是智能系统复杂环境真实应用中所必须解决的问题。
技术实现思路
(一)要解决的技术问题本专利技术要解决的技术问题是：如何设计一种无导航卫星信号下的自主定位方法，实现智能系统在无导航卫星信号环境下的准确自主定位。(二)技术方案为了解决上述技术问题，本专利技术提供了一种无导航卫星信号下的自主定位方法，包括以下步骤：步骤1：采用NDT-OM地图生成方法初始化形成基于三维激光雷达扫描数据的NDT-OM局部地图；步骤2：采用NDT-D2D匹配方法将利用步骤1的方法生成的通过三维激光雷达扫描生成的当前时刻的新的局部地图与利用步骤1的方法生成的通过三维雷达扫描生成的前一时刻的局部地图进行匹配；步骤3：局部地图匹配完成后，融合里程计、INS获取的位姿估计信息与该局部地图匹配量信息，同步完成自主定位。优选地，步骤1中，所述NDT-OM地图生成方法为：将智能系统周围的空间规则地细分成大小一定的栅格单元ci，ci中测量点个数为N，其中第k个测量点为xk∈R3，ci位置处测量点的平均值为ci位置处测量点的协方差矩阵为ci表示为：ci＝{ui,Pi,Ni,p(mi|z1:t)}(1)其中，u为均值，Ni是估计正态分布参数所使用的点数，p(mi|z1:t)为栅格单元被占有的概率，其利用平均值和协方差矩阵建立的正态分布模型来度量某个位置样本zk位于栅格单元ci内的概率，即：对于三维测量数据，每个NDT-OM三维空间栅格单元中需要保存的最少参数为11个，即x、y、z三个方向的3个平均值和6个协方差、栅格单元ci中的激光点数量、栅格单元ci被占用的概率值；对于新获得的测量值信息，系统将更新每个栅格单元的概率值进行地图更新，假设有两组测量点值和m，n均为小于或等于N的正整数，采用下列公式对其进行融合，得到新的栅格单元概率值：ux为x的均值，uy是y的均值,令则组合后的协方差估计值为：其中，经推导可得：以上更新方法未考虑环境动态变化的情况，为此增加射线跟踪步骤，其目的是评估位于传感器原点(传感器射线发射中心)和测量点之间直线上的所有栅格单元，对于不包含测量点的栅格单元则计数值加1，并利用以下对数概率公式对栅格单元占用概率进行估计：其中，l(mk|z1:t-1)是在获得1到t-1时刻观测的情况下占用栅格mk的概率，Np是zt数据集合中落入栅格单元的数量，Ne是栅格单元被观察为空的次数，po为栅格单元当被测量为占有时的占有概率，pe为栅格单元被观察为空时的占有概率。优选地，步骤1中NDT-OM地图生成的步骤概括为：(1)采集三维测量数据，更新测量数据对象；(2)利用测量数据对象更新地图栅格单元；(3)重新计算各栅格单元模型参数；(4)返回(1)。优选地，步骤2中，所述NDT-D2D匹配方法是通过最小化两个三维激光雷达数据形成的NDT-OM局部地图之间的距离L2，实现当前时刻与前一时刻局部地图的匹配。优选地，步骤2中，通过最小化两个三维激光雷达数据形成的NDT-OM局部地图之间的距离L2，实现当前时刻与前一时刻局部地图的匹配的过程为：假设一组参考激光雷达扫描点为F，附近位置处的一组动态扫描点为Μ，MNDT(F)与MNDT(Μ)分别指对这两组扫描点采用步骤1中利用NDT-OM的方式进行建模生成的局部地图，设T'为两张地图的平移方程，Θ为其参数，T'(Μ,Θ)即为两组扫描点的平移方程，则两张NDT-OM局部地图MNDT(F)与MNDT(Μ)之间的距离L2定义为：p表示概率密度；式(8)等价为：当方程(9)可以表示为Renyi’s二次熵之和时，即会有关两张地图的距离项，Renyi’s二次熵定义为：Hrqe(P(x))＝-∫p2(x)dx(10)获取的两张局部地图的最小距离项为：∫Ν(x|μi,∑i)Ν(x|μj,∑j)dx＝Ν(0|μi-μj,∑i+∑j)(11)其中，∑i，∑j为高斯分布的方差，i为MNDT(Μ)里的第i个栅格单元的序号，j为MNDT(F)里的第j个栅格单元的序号，N为高斯分布密度函数(方差是sigma)，x为变量，μi，μj分别为两张地图对应高斯模型的均值，将该距离项最小化，即会获取两张局部NDT-OM地图的最小距离L2的最终表达式为：MNDT(Μ)、MNDT(F)中分别有n个栅格单元，Μ(n)表示MNDT(Μ)里第n个栅格单元，F(n)表示MNDT(F)里第n个栅格单元，T(·,Θ)为平移变化矩阵，旋转矩阵为R，将其用欧拉角参数化来表示，转换矩阵的可以用向量p＝{tx,ty,tz,rx,ry,rz}来表示，t＝{tx,ty,tz}为平移参数，rx、ry、rz为三个方向的欧拉角，则获取两张NDT-OM局部地图的最小距离L2方程为：其中，d1、d2为设置的扫描参数，μij(p)为平均的矢量距离，即μij(p)＝Rμi+t-μj；如果距离L2小于预先设置的阈值，则基于NDT融合算法，用新的扫描数据更新该两张局部地图，而后继续处理新的数据；如果距离L2大于预先设定的阈值，则初始化一个新的NDT-OM局部地图，每一时刻计算的是当前时刻局部地图和前一刻共2个局部地图。优选地，步骤2中，如果有多个局部地图，则通过批处理扩展卡尔曼滤波方法融合里程计、INS信息，获取智能系统的位置估计值，以获取两张局部NDT-OM地图之间的相对位置约束，即局部地图原点之间的距离，如果该距离小于设定的阈值，则再次利用NDT-D2D算法对两个局部地图进行匹配，如果地图匹配成功，则给图增加新的相对位置约束，直至闭环。优选地，还包括步骤4：进行闭环探测：进行地图的索引值评估，当两个索引值超过阈值，则认为探测到了闭环。优选地，还包括步骤5：探测到闭环后，进行SLAM后端优化，采用iSAM库实现，优化SLAM中的稀疏非线性问题。本专利技术还提供了一种所述方法在无人车
中的应用。本专利技术还提供了一种所述方法在机器人
中的应用。(三)有益效果本专利技术采用正态分布变换同步定位与地图生成(NDT-SLAM)的自主定位方法，通过正态分布变换占用地图(NDT-OM)方法形成基于三维激光雷达扫描数据的NDT-OM局部地图，并通过NDT-D2D匹配方法完成局部NDT-OM地图之间的匹配，并在此基础上融合由惯性导航系统(INS)本文档来自技高网...

【技术保护点】
1.一种无导航卫星信号下的自主定位方法，其特征在于，包括以下步骤：/n步骤1：采用NDT-OM地图生成方法初始化形成基于三维激光雷达扫描数据的NDT-OM局部地图；/n步骤2：采用NDT-D2D匹配方法将利用步骤1的方法生成的通过三维激光雷达扫描生成的当前时刻的新的局部地图与利用步骤1的方法生成的通过三维雷达扫描生成的前一时刻的局部地图进行匹配；/n步骤3：局部地图匹配完成后，融合里程计、INS获取的位姿估计信息与该局部地图匹配量信息，同步完成自主定位。/n

【技术特征摘要】
1.一种无导航卫星信号下的自主定位方法，其特征在于，包括以下步骤：
步骤1：采用NDT-OM地图生成方法初始化形成基于三维激光雷达扫描数据的NDT-OM局部地图；
步骤2：采用NDT-D2D匹配方法将利用步骤1的方法生成的通过三维激光雷达扫描生成的当前时刻的新的局部地图与利用步骤1的方法生成的通过三维雷达扫描生成的前一时刻的局部地图进行匹配；
步骤3：局部地图匹配完成后，融合里程计、INS获取的位姿估计信息与该局部地图匹配量信息，同步完成自主定位。


2.如权利要求1所述的方法，其特征在于，步骤1中，所述NDT-OM地图生成方法为：
将智能系统周围的空间规则地细分成大小一定的栅格单元ci，ci中测量点个数为N，其中第k个测量点为xk∈R3，ci位置处测量点的平均值为ci位置处测量点的协方差矩阵为ci表示为：
ci＝{ui,Pi,Ni,p(mi|z1:t)}(1)
其中，u为均值，Ni是估计正态分布参数所使用的点数，p(mi|z1:t)为栅格单元被占有的概率，其利用平均值和协方差矩阵建立的正态分布模型来度量某个位置样本zk位于栅格单元ci内的概率，即：



对于三维测量数据，每个NDT-OM三维空间栅格单元中需要保存的最少参数为11个，即x、y、z三个方向的3个平均值和6个协方差、栅格单元ci中的激光点数量、栅格单元ci被占用的概率值；
对于新获得的测量值信息，系统将更新每个栅格单元的概率值进行地图更新，假设有两组测量点值和m，n均为小于或等于N的正整数，采用下列公式对其进行融合，得到新的栅格单元概率值：



ux为x的均值，uy是y的均值,令则组合后的协方差估计值为：



其中，
经推导可得：



以上更新方法未考虑环境动态变化的情况，为此增加射线跟踪步骤，其目的是评估位于传感器原点(传感器射线发射中心)和测量点之间直线上的所有栅格单元，对于不包含测量点的栅格单元则计数值加1，并利用以下对数概率公式对栅格单元占用概率进行估计：



其中，l(mk|z1:t-1)是在获得1到t-1时刻观测的情况下占用栅格mk的概率，Np是zt数据集合中落入栅格单元的数量，Ne是栅格单元被观察为空的次数，po为栅格单元当被测量为占有时的占有概率，pe为栅格单元被观察为空时的占有概率。


3.如权利要求1所述的方法，其特征在于，步骤1中NDT-OM地图生成的步骤概括为：
(1)采集三维测量数据，更新测量数据对象；
(2)利用测量数据对象更新地图栅格单元；
(3)重新计算各栅格单元模型参数；
(4)返回(1)。


4.如权利要求2所述的方法，其特征在于，步骤2中，所述NDT-D2D匹配方法是通过最小化两个三维激光雷达数据形成的NDT-OM局部地图之间的距离L2，实现当前时刻与前一时刻局部地图的匹配。


5.如权利要求4所述的方法，其特征在于，步骤2中，通过最小化两个三维激光雷达数据形成的NDT-OM局部地图之间的距离L2，实现当前时刻与前一时刻局部地...

【专利技术属性】
技术研发人员：康晓，吴越，靳璐，靳保，海丹，罗涛，谢强，苏治宝，苏波，
申请(专利权)人：中国北方车辆研究所，
类型：发明
国别省市：北京;11