CN108195376B - 小型无人机自主导航定位方法 - Google Patents

Abstract

本发明属于无人机技术领域，具体涉及一种小型无人机自主导航定位方法，其利用改进建议分布粒子滤波实现无人机的同步定位与地图构建方法框架的设计。并针对无人机运动模型、观测模型线性化导致的模型不匹配及粒子的退化问题加以研究。提出利用自适应EKF及RTS产生建议分布，融入最新的观测量，设计无人机的同步定位与构图方法，产生更接近真实分布的粒子集，通过重采样，对粒子的状态进行更新，从而从根本上避免模型线性化的影响，提高估计精度，同时由于不需要计算系统状态矩阵的协方差，降低了计算量，最终达到提高无人机长航时导航定位与地图构建精度及效率的目的。

Description

小型无人机自主导航定位方法

技术领域

本发明属于无人机技术领域，具体涉及一种小型无人机自主导航定位方法。

背景技术

现代社会人力资源成本价值越来越高，尤其对于环境比较恶劣或者比较耗时的任务，例如危险的楼内火灾救援现场，或枯燥耗时的楼内墙壁粉刷等，利用机器来替代人工是非常不错的选择，于是各种小型无人机应运而生，而由于自主导航定位技术是小型无人机完成任何任务的先决条件，所以导航定位技术成为限制小型无人机发展的瓶颈技术。

传统的小型无人机大都采取航位推算配合GPS的组合导航方法实现导航定位，随着时间推移，其位置估计误差增大，需要定期采用GPS或北斗对其位置进行校正。这种方法对于GPS或北斗的卫星信号要求比较高，一旦信号受到遮挡或者变弱，将无法进行位置校正，导致位置估计误差累计较大。

同步定位与构图(Simultaneous Localization and Mapping，SLAM)技术的提出，一定程度上缓解了传统导航方法中存在的不足。SLAM是在无人机自身初始位置不确定、航位推算过程中位置误差逐渐累积的情况下，利用自身携带的环境感知传感器反复探测环境中特征，从而完成自身及特征位置的校正，同时构建环境地图，无需借助外部辅助定位设备(GPS/北斗)即可以得到较为可靠的无人机位置信息及环境地图信息。

同步定位与构图技术的主要框架依赖于滤波估计方法，比较流行的是基于EKF框架。然而，EKF一般通过对系统方程进行一阶泰勒展开的方式实现非线性方程的线性化，这一操作不可避免的会产生线性化后的截断误差，而小型无人机的运动模型本来就不是精确的，这就势必导致其导航定位结果受到严重影响，并且由于需要对系统状态误差的协方差进行估计，导致其计算量较大，实时性受到影响；基于粒子滤波的SLAM框架是另一种比较流行的框架，但是传统的粒子滤波采用先验分布作为建议分布，没有考虑最新的测量，导致粒子退化问题严重，限制了无人机导航定位精度。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何将自适应滤波方法及RTS平滑过程与粒子滤波相结合以降低粒子退化程度，同时将这种改进的粒子滤波与SLAM方法相结合以解决无人机自主导航过程中存在的位置误差累积、线性化截断误差及粒子退化等问题。

(二)技术方案

为解决上述技术问题，本发明提供一种小型无人机自主导航定位方法，该方法包括如下步骤：

步骤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]表示以加速度的形式作用在速度上的高斯噪声；

根据特征状态

和无人机状态XA可以获得SLAM系统的状态X，包括无人机的状态XA和特征的状态X_f：

X＝[X_A X_f]^T (3)

1.5传感器测量模型

(1)速度测量模型

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

Z_V＝H_VX+s_V (4)

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

(2)高度测量模型

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

Z_H＝H_HX+s_H (5)

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

(3)航向测量模型

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

Z_C＝H_CX+s_C (6)

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

(4)环境测量模型

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

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

步骤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：地图扩张

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

(三)有益效果

与现有技术相比较，本发明是一种无人机仅通过自身携带的速度、姿态、环境感知传感器等实现自主导航的方法。导航定位是无人机完成预定使命的必备条件，是决定其能否自主完成任务的关键因素。本发明所述方法中，无人机摆脱室内GPS/北斗信号弱的限制及捷联惯导等造价高昂的外部导航设备的依赖，在运动的同时利用自身携带的环境感知传感器，不断探测结构化室内环境中的线特征，动态地增量式构建环境特征地图，同时利用对环境中特征的重复观测实现自身位置的校正，进而高效、快速的完成各种结构化环境中的任务。

具体而言，该技术方案中构建了无人机SLAM系统的地图模型、坐标系统、特征模型、无人机的运动模型以及传感器测量模型。利用改进建议分布粒子滤波实现无人机的同步定位与地图构建方法框架的设计。并针对无人机运动模型、观测模型线性化导致的模型不匹配及粒子的退化问题加以研究。提出利用自适应EKF及RTS产生建议分布，融入最新的观测量，设计无人机的同步定位与构图方法，产生更接近真实分布的粒子集，通过重采样，对粒子的状态进行更新，从而从根本上避免模型线性化的影响，提高估计精度，同时由于不需要计算系统状态矩阵的协方差，降低了计算量，最终达到提高无人机长航时导航定位与地图构建精度及效率的目的。

附图说明

图1为无人机同步定位与构图系统坐标系示意图。

图2为基于粒子滤波框架的无人机同步定位与构图示意图

图3为所设计发明中粒子状态描述示意图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

为解决上述技术问题，本发明提供一种小型无人机自主导航定位方法，该方法包括如下步骤：

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

1.1地图模型

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

1.2坐标系统

由于无人机所携带的传感器与环境特征之间的感知行为发生在传感器坐标系中，速度的测量发生在无人机运动坐标系中，而最终环境地图的构建需要表示在全局地图中，所以建立全局坐标系，无人机运动坐标系及传感器坐标系，并表明他们之间的关系，如图1所示，定义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]表示以加速度的形式作用在速度上的高斯噪声；

根据特征状态

和无人机状态XA可以获得SLAM系统的状态X，包括无人机的状态XA和特征的状态X_f：

X＝[X_A X_f]^T (3)

1.5传感器测量模型

(1)速度测量模型

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

Z_V＝H_VX+s_V (4)

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

(2)高度测量模型

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

Z_H＝H_HX+s_H (5)

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

(3)航向测量模型

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

Z_C＝H_CX+s_C (6)

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

(4)环境测量模型

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

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

步骤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：地图扩张

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

实施例1

本实施例提供一种基于改进建议分布粒子滤波的无人机同步定位与构图方法，如图1-图3所示，该方法包括如下步骤：

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

1.1地图模型

目前比较流行的地图模型主要有栅格地图、拓扑地图以及特征地图。本发明所设计方法主要针对结构化室内环境，而室内环境中最显著的特征是墙面之间相交的线特征，所以本发明采用特征地图模型；

1.2坐标系统

由于无人机所携带的传感器与环境特征之间的感知行为发生在传感器坐标系中，速度的测量发生在无人机运动坐标系中，而最终环境地图的构建需要表示在全局地图中，所以建立全局坐标系，无人机运动坐标系及传感器坐标系，并表明他们之间的关系，详细如图1所示，定义GX_GY_GZ_G为全局坐标系，G为地心，X_G指向正北，Y_G指向正东，Z_G垂直于X_GY_G平面并指向G；定义AX_AY_AZ_A为无人机运动坐标系，A为无人机质心，X_A指向机头，Y_A垂直于机头，Z_A垂直于X_AY_A平面并指向A；定义SX_SY_SZ_S为环境感知传感器坐标系，S为传感器质心，X_S指向传感器电磁波发射方向，Y_S垂直于电磁波发射方向，Z_S垂直于X_SY_S平面并指向S；三个坐标系均符合右手定则；

1.3特征模型

由于本发明主要针对室内静态结构化环境，所以各种类型的特征的位置是不变的，即后一时刻的状态和前一时刻的状态相同，所以根据公式(1)，定义线特征的运动学模型为：

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

特征在传感器坐标系中一般以极坐标的形式表示：

其中，

表示第i个特征的状态，即位置；n为特征的个数；

ρ_i为特征i在极坐标系下的极径；θ_i为特征i在极坐标系下的极角；

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中相应的X_A、Y_A、Z_A方向的线速度和转艏角速度，n＝[n_u,n_v,n_w,n_r]表示以加速度的形式作用在速度上的高斯噪声；

根据特征状态

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

X＝[X_A X_f]^T (3)

其中，T为采样时间；

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为特征测量噪声；

其中，

和

是线特征在运动坐标系下的参数表示，ρ_i和θ_i是点特征在全局坐标系下的参数表示；

步骤S2：基于步骤S1所建立的无人机SLAM系统模型，执行自适应EKF的无人机同步定位与构图操作；

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

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

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

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

步骤B：粒子状态预测：

其中F(k)是非线性运动方程f对状态XV在当前最优估计处的雅克比矩阵，W(k)是非线性状态方程f对噪声m(k)的雅克比矩阵。

由时变噪声统计特性估值器递推获得：

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

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

步骤D：粒子状态更新：

步骤D-1：如果数据关联结果表明该次测量为地图中已有特征，生成AEKF-RTS建议分布，融入最新的观测量，对粒子进行重要性采样；

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

P(k)＝[I-K(k)H(k)]P(k|k-1) (13)

K(k)＝P(k|k-1)H(k)^T(S(k))^-1S(k) (14)

其中，H是非线性传感器观测方程h对X的偏导数的雅克比矩阵，H根据测量传感器的不同而具有相应的不同形式；

和

是由时变噪声统计估值器递推获得：

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

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

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

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

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

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

对于每一时刻k＝T-1,T-2,…0,状态及方差进行平滑修正，

其中

为k时刻的平滑增益，

和

分别为AEKF滤波状态的一步预测及协方差，

和

分别为AEKF滤波更新后的状态估计值和协方差。

和P^S(k|T)分别为RTS平滑修正后k时刻的状态向量及其协方差。可知，平滑修正由

逐步逆向递推，依次得到：

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

根据对各个时刻的状态向量及协方差进行预测更新及平滑修正得到的状态估计值及协方差序列为：

从而构造AEKF-RTS建议分布函数如下：

在滤波过程之后进行平滑修正，能够缓解平滑滞后问题，实施分段平滑，

取平滑步长T_S＝T/3，从而边平滑边生成建议分布函数。

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

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

步骤E：地图扩张

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

利用上述技术方案，采用上述操作步骤，本发明可以解决无人机长航时、室内结构化环境中缺少GPS情况下的导航定位问题，利用自身携带的环境感知传感器对地面特征进行测量，从而实现自身位置的预测校正以及地面特征位置的校正过程，无需定期利用GPS数据进行位置校正并构建地面环境地图。使得无人机能够摆脱地面人员的控制操作，从而实现真正自主执行任务。该方法已经仿真验证，效果理想。

实施例2

本实施例提供一种基于改进建议分布粒子滤波的无人机同步定位与构图方法，包括如下步骤：

(1)地图模型建立；

(2)坐标系统建立；

(3)特征模型建立；

(4)无人机运动模型建立；

(5)传感器测量模型；

(6)提取环境感知传感器所获取图像中的特征；

(7)完成新观测特征与地图中已存在特征之间的数据关联过程；

(8)完成新特征的融入地图。或者利用已观测特征实现对无人机位置和地图中已存在特征位置的校正。

其中，所构建的无人机SLAM系统模型及观测模型是非线性的。本发明采用粒子来存储无人机的航行状态及观测的环境特征状态，通过对一组粒子集的状态进行预测，然后采用自适应EKF融入最新的观测值并经过RTS平滑，产生更优的建议分布，并对从此建议分布中获取的粒子进行重采样，进而对粒子状态进行更新，最终实现无人机的同步定位与构图过程。此发明可提高SLAM过程中无人机位置及地图中特征位置估计的精度，对于无人机长航时、缺少GPS信号的情况下执行任务具有重要意义。

综上，本发明建立了无人机同步定位与构图系统的地图模型、坐标系统、特征模型、无人机的运动模型以及传感器测量模型。利用改进的粒子滤波实现无人机的同步定位与地图构建方法框架的设计。并针对无人机在自主导航过程中存在的位置误差累计、系统模型非线性以及采用先验分布作为建议分布粒子退化现象严重的问题加以研究。提出利用自适应EKF融入最新的测量值并经过RTS平滑产生改进的改进建议分布，设计基于改进建议分布的粒子滤波，并基于改进的粒子滤波框架设计同步定位与构图方法，从而避免模型线性化的影响，降低误差累计程度，而且有效避免粒子退化问题，从而达到提高无人机长航时导航定位与地图构建精度的目的。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (1)

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：地图扩张

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

Images