[发明专利]一种基于FastSLAM算法的无人机着陆方法

摘要

本发明公开了一种基于FastSLAM算法的无人机着陆方法，属于导航技术领域。所述方法包括以下几个步骤：步骤一，建立无人机着陆段系统模型；步骤二，设计无人机着陆段FastSLAM算法；步骤三，采用反正切跟踪微分器获得速度估计。本发明利用FastSLAM算法，分别采用粒子滤波和扩展卡尔曼滤波对无人机路径和环境特征进行估计，构建环境地图，实现无人机对自身定位的更新，实现自主导航,具有导航精度高的特点，能够满足无人机着陆段的定位精度及实时性要求。

主权项

1.一种基于FastSLAM算法的无人机着陆方法，其特征在于，包括以下几个步骤：步骤一，在东北天地理坐标系下，建立无人机着陆段系统模型，所述的系统模型包括无人机的运动模型和观测模型，具体表示为：z＝h(x(t),m(t),t)+εt    (2)其中，t为时间，x(t)表示t时刻的无人机的状态向量，表示无人机状态向量的微分，无人机的状态向量x＝[x,y,z,ψ,θ]^T,其中x,y,z分别表示无人机在地面坐标系oxyz下的三轴位置坐标，其中o是地面上选取的一点，x,y,z分别指向东、北、天方向，ψ为无人机速度矢量在xoy平面投影与x轴的夹角，θ为无人机速度矢量与xoy平面的夹角,统称为无人机速度方位角；在无人机着陆航迹附近分布若干环境路标，利用机载激光雷达获得无人机和环境路标之间的距离以及方位角；z表示无人机对环境路标的观测量，δ_t表示运动模型中的高斯噪声，运动模型协方差矩阵为Q_t,ε_t是观测模型中的观测噪声，观测模型协方差矩阵为R_t，u(t)＝v是机载惯导获得的t时刻的无人机速度，m(t)为环境路标在地面坐标系下的位置向量，运动模型离散化后形式如下：l是指第l个采样步，Δt是采样时间间隔，Δθ是相邻两个采样步中θ的增量，Δψ是相邻两个采样步中ψ的增量，δt1、δt2、δt3、δt4、δt5分别表示无人机各状态量x、y、z、ψ和θ对应的高斯噪声，v表示机载惯导获得的当前时刻无人机的速度大小；在无人机着陆航迹上选择若干个航点，设初始待达航点为第一个航点，计算无人机当前位置和待达航点之间距离，若小于设定阈值Rmin，则待达航点变为下一个航点；速度方位角增量Δθ,Δψ计算如下：其中wpx,wpy,wpz分别是待达航点的地面坐标系下的位置坐标，x(l),y(l),z(l),ψ(l),θ(l)分别为第l个采样步下无人机的状态量；无人机对环境路标的观测量的向量形式表示为z＝[r,α,β]T,具体形式如下：其中，r为无人机和环境路标之间的距离，α是无人机与环境地标之间位置矢量与无人机速度矢量分别与水平面的夹角之差，β是无人机与环境地标之间位置矢量在xoy面上的投影与x轴夹角与速度方位角ψ之差，x,y,z分别为无人机的当前位置坐标，xm,ym,zm分别为环境路标的三轴位置坐标；步骤二，设计无人机着陆段FastSLAM算法；具体包括以下几个步骤：1)无人机状态更新；使用N个粒子滤波器对无人机状态进行估计，初始化粒子权重系数为1/N，初始化航点和环境路标，初始状态向量为x0＝[x0,y0,z0,ψ0,θ0]T，x0的各元素分别为无人机初始位置以及初始速度的方位角；预测无人机下一个采样步的理想状态：其中，x(l+1),y(l+1),z(l+1),ψ(l+1),θ(l+1)分别为无人机第l+1个采样步的预测状态；对速度和速度方位角增量添加过程噪声，对每个粒子，进行状态递推，有如下第l步和第l+1步状态向量之间的关系：其中，xp,yp,zp分别为粒子当前的位置，ψp,θp为粒子当前的速度方位角；vn是添加随机过程噪声后的速度，Δψn是添加随机过程噪声后的方位角ψ的增量，Δθn是添加随机过程噪声后的方位角θ的增量；如果达到观测周期，机载激光雷达对环境路标进行探测，获得激光雷达探测距离内的所有环境路标观测量以及环境路标序列，存储已探测到的环境路标序列，若是第一次探测到的环境路标，环境路标观测量存储在新路标集zn中，若不是第一次探测到的环境路标，则环境路标观测量存储在非新路标集zf中；否则，重新获得无人机预测状态；2)更新环境路标；若非新路标集zf不为空集，对非新路标集zf中的环境路标，计算环境路标观测量的预测值，首先给出中间变量dx,dy,dz,d和d1如下：其中x_p,y_p,z_p是粒子状态值中的位置分量，为环境路标观测量的预测值，x_f,y_f,z_f分别为非新路标集z_f中环境路标的三轴位置坐标；采用扩展卡尔曼滤波对环境路标的均值和方差进行更新，得：其中，下脚标t表示t时刻，下脚标f表示针对的是非新路标集；Rt为观测噪声的协方差矩阵，Kt是卡尔曼滤波的滤波增益矩阵，Hf为观测方程(2)对环境路标坐标(xf,yf,zf)的雅克比矩阵，I是维数为3的单位矩阵，Pf,t是环境路标的协方差阵，μt为环境路标的均值，zt为环境路标观测量；所述的雅克比矩阵Hf表示为：定义：Lt＝HfPf,tHfT+Rt    (12)第k个粒子的权重系数如下：上标[k]表示第k个粒子，k范围是1～N，z是无人机对环境路标的观测向量；若新路标集zn不为空集，表明观测到新的环境路标，则需要扩展环境地图；设第k个粒子的状态向量为x[k]＝[xp[k],yp[k],zp[k],ψp[k],θp[k]]T，对于新路标集zn中的观测量z＝[r,α,β]T，定义中间变量：令xf＝[xp[k]+Δx,yp[k]+Δy,zp[k]+Δz]T    (16)Pf,t＝Hz*Rt*HzT    (17)Pf,t是新增环境路标的协方差阵，xf是环境路标位置的估计值，r为无人机和环境路标之间的距离，Δx是环境路标估计值的x轴分量与粒子状态的x轴分量之差，Δy是环境路标估计值的y轴分量与粒子状态的y轴分量之差，Δz是环境路标估计值的z轴分量与粒子状态的z轴分量之差；3)粒子重采样；式(13)已经获得了第k个粒子的权重系数现在要计算每个粒子的均一化权重系数，第k个粒子的归一化权重系数如下：计算有效粒子数设置有效粒子数阈值N_min，如果Neff＜N_min，则进行重采样步骤，求出重采样后粒子状态均值进而获得无人机位置估计；否则不需要重采样；步骤三，采用反正切跟踪微分器获得速度估计。