[发明专利]一种基于改进粒子滤波的移动机器人定位方法

摘要

本发明提供了一种基于改进粒子滤波的移动机器人定位方法，包括步骤：建立机器人的运动方程和路标计算方程；用多agent粒子群优化算法优化粒子集，所得的最优值为对位姿的估计；利用卡尔曼滤波算法对环境路标进行估计；权重更新及归一化，重采样。本发明定位方法定位准确且易于实现，在移动机器人的仿真过程中，移动机器人的位姿估计和环境路标估计更加精确。

主权项

1.一种基于改进粒子滤波的移动机器人定位方法，其特征在于包括以下步骤：101、初始化移动机器人的工作条件，包括设定移动机器人的运动路径、移动方式、移动的起始位置和目标位置、路标的位置信息，获取移动机器人车轮上的光电编码盘△t时间内的转动次数n及光电编码盘的分辨率p，根据公式d=2πrn/p求得移动机器人在△t时间内的移动距离d，移动机器人的左、右轮移动距离分别设置为d₁和d₂，根据公式ΔD=d1+d22Δθ=d1-d2l]]>求得机器人的位移增量△D和转角增量△θ，其中r为车轮半径，l为两轮轮距，根据公式求得移动机器人在△t时间内的运动半径R，在获得以上参数后预测移动机器人在t+1时刻的位姿:X(t+1)=xt+ΔDtΔθtcosθtyt+ΔDtΔθtsinθtθt+θt+N(t)---(I)]]>式中x_t，y_t，θ_t分别为机器人在平面坐标体系下t时刻的横坐标、纵坐标及姿态角，姿态角即为机器人与横坐标的夹角，N(t)为因为车轮变形、地面摩擦形成的过程噪声，服从均值为零的高斯分布,t的取值为[1n],其中N(1)+N(2)+…+N(n)=0;102、根据路标坐标的估计方程式对路标坐标进行估计，其中机器人t时刻的位姿为X_t=(x_t,y_t,θ_t)，x_t，y_t，θ_t分别为机器人在平面坐标体系下t时刻的横坐标、纵坐标和姿态角，路标M的坐标为(x_m,y_m)，传感器位于机器人的前端，传感器中心不与机器人几何中心重合时，传感器中心坐标为(x_s,y_s)，传感器中心与机器人几何中心距离为L，根据公式xsys=xt+Lcosθtyt+Lsinθt]]>求得，路标坐标的观测值ρ_t为路标坐标与传感器中心坐标间的距离，为传感器观测到路标时的扫描角,扫描角为路标坐标与机器人几何中心的连线与横坐标正方向的夹角；观测值的估计方程为：103、将机器人某一时刻位姿视为若干个粒子组合形成粒子集，根据步骤101中得到的移动机器人的运动方程（Ⅰ）推出下一时刻的粒子集，用MAPSO优化算法对粒子集进行优化，并设定粒子集的更新权值W设，将优化后的值作为对机器人位姿的最终估计值；104、根据步骤102中得出的机器人路标坐标估计方程及步骤103中得到的机器人位姿的最终估计值，采用卡尔曼滤波算法对机器人的路标坐标进行迭代更新，得出路标坐标的最优值；根据粒子集的权值更新公式W_t+1=W_t*exp(-0.5*ρ_t²)，ρ_t为路标坐标与传感器中心坐标间的距离，当权值小于步骤103中所设定的更新权值W设时，返回步骤101，直至机器人的路标坐标的最优值和位姿的最终估计值确定，机器人定位结束。