[发明专利]小型无人机自主导航定位方法

权利要求书

1.一种小型无人机自主导航定位方法，其特征在于，该方法包括如下步骤：

步骤S1：对无人机SLAM系统建模；

1.1地图模型

采用特征地图来构建环境地图模型，将环境中的目标用几何原型来描述；

1.2坐标系统

由于无人机所携带的传感器与环境特征之间的感知行为发生在传感器坐标系中，速度的测量发生在无人机运动坐标系中，而最终环境地图的构建需要表示在全局地图中，所以建立全局坐标系，无人机运动坐标系及传感器坐标系，并表明他们之间的关系，定义GXGYGZG为全局坐标系，G为地心，XG指向正北，YG指向正东，ZG垂直于XGYG平面并指向G；定义AXAYAZA为无人机运动坐标系，A为无人机质心，XA指向机头，YA垂直于机头，ZA垂直于XAYA平面并指向A；定义SXSYSZS为环境感知传感器坐标系，S为传感器质心，XS指向传感器电磁波发射方向，YS垂直于电磁波发射方向，ZS垂直于XSYS平面并指向S；三个坐标系均符合右手定则；

1.3特征模型

根据公式(1)，定义特征的运动学模型为：

其中，k为离散时间变量；

1.4无人机运动模型

通过建立一个四自由度的常速运动学模型来描述无人机的运动变换趋势，如公式(2)所示：

X_A(k)＝f(X_A(k-1),n(k-1)) (2)

无人机X_A＝[x y z ψ u v w r]^T，[x,y,z,ψ]为全局坐标系中无人机的位置和艏向，[u,v,w,r]表示运动坐标系A中相应的XA、YA、ZA方向的线速度和航向角速度，n＝[n_u,n_v,n_w,n_r]表示以加速度的形式作用在速度上的高斯噪声；

根据特征状态和无人机状态X_A获得SLAM系统的状态X，包括无人机的状态X_A和特征的状态X_f：

X＝[X_A X_f]^T (3)

1.5传感器测量模型

(1)速度测量模型

速度测量传感器可提供X、Y、Z三个方向的速度，测量模型为：

Z_V＝H_VX+s_V (4)

其中，Z_V为速度的测量值，s_V为速度测量噪声；

(2)高度测量模型

通过高度测量传感器获得无人机的高度，测量模型为：

Z_H＝H_HX+s_H (5)

其中，Z_H为高度的测量值，s_H为高度测量噪声；

(3)航向测量模型

通过艏向测量传感器获得无人机的航向，测量模型为：

Z_C＝H_CX+s_C (6)

其中，Z_C为航向的测量值，s_C为航向测量噪声；

(4)环境测量模型

通过环境感知传感器获得环境中特征相对于无人机的距离和方位，测量模型为：

其中，s_i为特征测量噪声；

步骤S2：基于步骤S1所建立的无人机SLAM系统模型，执行基于自适应EKF-RTS建议分布粒子滤波的无人机同步定位与构图操作；

其中，建立无人机SLAM系统的状态方程和观测方程如下所示，被估计状态以及观测变量与状态向量的关系呈现非线性：

X(k)＝f(X(k-1),m(k))

Z(k)＝h(X(k),n(k)) (8)

式中，X(k)是无人机SLAM系统的状态向量；Z(k)是系统的观测向量；m(k)是系统的过程噪声序列；n(k)是系统的观测噪声序列；f()和h()表示系统的非线性状态方程和非线性观测方程；

通过对粒子滤波的建议分布进行改进，获得更加接近粒子真实分布的粒子集合，通过重采样，计算新的粒子权值，并获得更新后的粒子状态；进而按照下述基于自适应EKFRTS平滑粒子滤波SLAM，即EKF-RTS-PF-SLAM的基本步骤流程执行：

步骤A：初始化含有无人机状态及特征状态的粒子集；

步骤B：粒子状态预测：

步骤C：传感器观测及数据关联

首先获得速度、姿态及环境感知传感器观测值，如果数据关联结果表明所观测特征为地图中已存在特征，则根据对该特征的实际测量值与预测测量值之间的差异来实现无人机状态及整个地图中所有特征位置的校正；具体如下：

步骤C-1：生成AEKF-RTS建议分布，融入最新的观测量，对粒子进行重要性采样；

步骤C-1-1：AEKF粒子状态更新

步骤C-1-2：滤波估计状态值存储：

存储时间区域[0，T]内各时刻的滤波结果用于下一步的最优平滑；

步骤C-1-3：RTS平滑：

步骤C-1-3-1：初始化

利用k＝T时刻的滤波估计状态值初始化平滑器的状态及方差：

步骤C-1-3-2：平滑修正；

步骤C-1-4：AEKF-RTS建议分布生成；

根据对各个时刻的状态向量及协方差进行预测更新及平滑修正得到的状态估计值及协方差序列为：从而构造AEKF-RTS建议分布函数如下：

步骤C-2：计算粒子权值，对粒子状态进行更新；

步骤C-3：进行重采样，提取权值大于设定阈值的粒子；

步骤D：地图扩张

如果数据关联结果表明该次测量为新线特征，则进行地图扩张。