[发明专利]一种无导航卫星信号下的自主定位方法

权利要求书

1.一种无导航卫星信号下的自主定位方法，其特征在于，包括以下步骤：

步骤1：采用NDT-OM地图生成方法初始化形成基于三维激光雷达扫描数据的NDT-OM局部地图；

步骤2：采用NDT-D2D匹配方法将利用步骤1的方法生成的通过三维激光雷达扫描生成的当前时刻的新的局部地图与利用步骤1的方法生成的通过三维雷达扫描生成的前一时刻的局部地图进行匹配；

步骤3：局部地图匹配完成后，融合里程计、INS获取的位姿估计信息与该局部地图匹配量信息，同步完成自主定位。

2.如权利要求1所述的方法，其特征在于，步骤1中，所述NDT-OM地图生成方法为：

将智能系统周围的空间规则地细分成大小一定的栅格单元c_i，c_i中测量点个数为N，其中第k个测量点为x_k∈R³，c_i位置处测量点的平均值为c_i位置处测量点的协方差矩阵为c_i表示为：

c_i＝{u_i,P_i,N_i,p(m_i|z_1:t)} (1)

其中，u为均值，N_i是估计正态分布参数所使用的点数，p(m_i|z_1:t)为栅格单元被占有的概率，其利用平均值和协方差矩阵建立的正态分布模型来度量某个位置样本z_k位于栅格单元c_i内的概率，即：

对于三维测量数据，每个NDT-OM三维空间栅格单元中需要保存的最少参数为11个，即x、y、z三个方向的3个平均值和6个协方差、栅格单元c_i中的激光点数量、栅格单元c_i被占用的概率值；

对于新获得的测量值信息，系统将更新每个栅格单元的概率值进行地图更新，假设有两组测量点值和m，n均为小于或等于N的正整数，采用下列公式对其进行融合，得到新的栅格单元概率值：

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

其中，

经推导可得：

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

其中，l(m_k|z_1:t-1)是在获得1到t-1时刻观测的情况下占用栅格m_k的概率，N_p是z_t数据集合中落入栅格单元的数量，N_e是栅格单元被观察为空的次数，p_o为栅格单元当被测量为占有时的占有概率，p_e为栅格单元被观察为空时的占有概率。

3.如权利要求1所述的方法，其特征在于，步骤1中NDT-OM地图生成的步骤概括为：

(1)采集三维测量数据，更新测量数据对象；

(2)利用测量数据对象更新地图栅格单元；

(3)重新计算各栅格单元模型参数；

(4)返回(1)。

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