[发明专利]一种基于改进的蒙特卡洛算法的自主学习全局定位方法

权利要求书

1.一种基于改进的蒙特卡洛算法的自主学习全局定位方法，其包括以下步骤：

步骤一：获取地图，创建粒子滤波器；

步骤二：读取当前激光和已知地图，利用公式(1)进行扫描匹配，在地图上获得最优初始位姿；其中，p是机器人的位姿坐标，L_i(p)是在机器人p位姿下，对应第i个激光点的坐标，W(L_i(p))表示这些激光点落在栅格地图中占据格子(Occupied)里的概率：

步骤三：利用步骤二中的位姿初始化粒子群，更新粒子群至收敛或超时；

步骤四：判断是否有激光输入，如果没有，则等待；若有，判断机器人是否移动，当里程计大于阈值时或角度大于阈值时，利用公式2-公式10里程采样运动模型算法进行滤波器预测，在机器人运动学约束下的可行区域内高斯采样；

其中α₁、α₂、α₃、α₄是里程模型误差参数，[x′,y′,θ′]为预测样本的位姿；

步骤五：滤波器更新；包括以下步骤：

a.读取当前时刻的激光数据，用公式11将激光数据转换到机器人坐标系下；

其中，θ是机器人的偏航角，l_i是指在机器人位姿p下，第i个激光点的原始数据，s_x和s_y分别表示在机器人坐标系下，激光雷达位置的坐标；

接着，将转换后的坐标投影到地图上，并通过似然模型算法中公式12计算出最近邻占据点的距离d；

其中，和分别表示激光投影到地图上的坐标，x′,y′表示占据格子的坐标，m表示地图；

利用公式13计算激光粒子击中占据格子的概率；

其中，prob(d,σ_hit)是零均值下，σ_hit为标准差的高斯函数，z_hit、z_random和z_max分别为测量误差的权重参数；

用公式11-公式13，遍历所有的激光点，为每个粒子计算出一个得分；

b.为了保证定位的实时性，采用并行计算，开辟一个扫描匹配的线程，利用上一时刻最优位姿对应的激光数据和当前的地图进行扫描匹配，获取当前时刻最优的位姿；

步骤六：计算出最优位姿的得分并添加到粒子群中；

步骤七：对粒子群重采样和归一化，为了实现粒子数的自适应，使用KLD算法，计算出粒子群的总数；

所述KLD算法用公式15根据当前粒子得分的分布每次计算出粒子群的总数N；

其中，k为粒子状态空间中非空的数量，α为真是后验分布和估计分布之间的最大误差，β为以概率(1-σ)来确定的样本数；

步骤八：经过上述步骤，粒子滤波器完成了预测和更新，此时获取到当前时刻最优的位姿和方差；

步骤九：判断是否正在进行导航任务，如果是，开启自主学习进程；

步骤十：判断当前环境变化和地图差异是否超过阈值，当超过阈值时，阻塞自主学习线程，进入第一阶段，过滤掉不匹配的激光点，用剩下的激光点执行上述步骤一至七；当超过阈值时，阻塞自主学习线程，进入第二阶段，在数据库中查找与上一时刻机器人位姿最近邻的一个位姿，获取对应的激光帧；

步骤十一：判断当前环境变化和地图差异是否超过阈值，若没有超过，则通知自主学习线程继续运行，读取激光的数据执行步骤一至七。