基于四元数SRCKF-SLAM算法的植保无人机姿态估计

Attitude estimation for plant protect UAV based on quaternion SRCKF-SLAM algorithm

摘要

以传统四元数作为飞行器非线性状态模型描述参数，进行姿态估计时存在精度不足问题，提出了一种基于四元数的平方根容积卡尔曼滤波算法。该算法以姿态四元数误差及陀螺仪漂移误差为状态量，以SINS/SLAM组合导航的姿态四元数为观测量，并采用平方根容积卡尔曼滤波算法进行姿态估计，既解决了传统四元数的规范化问题，又降低了传统四元数的平方根UKF算法的状态维数及复杂度，提高了数值稳定性。与四元数SRUKF、四元数SRCDKF算法比较，仿真实验结果表明新算法估计横滚角、俯仰角、偏航角误差均值分别为{\mathrm{2}}^{″}、{\mathrm{3}}^{″}、{\mathrm{1}}^{″}，误差均最小，且算法精度较四元数SRUKF-SLAM算法提高了约30%，具有较高的滤波精度及数值稳定性，计算效率最优。

Abstract

The traditional quaternion served as the description parameter of the nonlinear state model of the UAV， and the accuracy of the attitude estimation was presented. A square-root cubature Kalman filter algorithm based on quaternion was proposed. The algorithm took the attitude quaternion error and the gyro drift error as the state quantities， and measured the attitude quaternion of SINS/SLAM navigation. The square root cubature Kalman filter algorithm was used for pose estimation， which not only can solve the normalization issue of traditional quaternion， but also can reduce the state dimension and computational complexity of the square-root UKF algorithm of traditional quaternion and improve the numerical stability. Compared with the quaternion-based SRUKF and quaternion SRCDKF algorithm， the simulation results showed that the new algorithm estimated the error mean values of the roll angle， pitch angle and yaw angle， which were {\mathrm{2}}^{″},{\mathrm{3}}^{″}，{\mathrm{1}}^{″}， respectively. The errors were the smallest， and algorithm’s accuracy was about 30% higher than that of the quaternion SRUKF-SLAM algorithm， and it exhibited high filtering accuracy and numerical stability and the lowest computational cost.

Graphical abstract

关键词

四元数 / 状态模型 / 平方根容积卡尔曼滤波 / 姿态估计 / 数值稳定性

Key words

quaternion / state model / square root cubature Kalman filter / attitude estimation / numerical stability

0 引言

对航空弹箭导航、制导与控制是实现高精度目标跟踪的重要技术指标，进一步对敌机目标跟踪与定位，其本质是对其状态估计及姿态估计问题^［1-2］。处理目标跟踪系统中的非线性状态模型时，通常采用姿态四元数作为载体的姿态模型参数^［3-4］，它较欧拉角姿态参数描述法具有全局维数小、非奇异等优点，且可实现目标全姿态跟踪。

对于非线性系统建模的三维姿态参数的奇异性问题及高维姿态参数解算的冗余性问题^［5-6］，以传统的四元数参数作为状态量的姿态估计建模并不能很好地解决上述问题。VATHSAL等最早提出了四元数的二阶扩展卡尔曼滤波算法（Q-Extended Kalman filter， QEKF），该算法存在较复杂的矩阵解算问题，导致滤波估计精度较低^［7］。为此，文献［8］提出了一种基于四元数的无迹卡尔曼滤波算法（Q-unscented Kalman filter， QUKF），但传统UKF算法的Sigma点选取量大、状态量维数高，且负权值Sigma点会导致算法协方差的非正定问题，所以该算法精度还需进一步改进。2009年，一种以N个采样点经非线性求容积规则变换后，产生一组全新的\mathrm{2}N个同权值cubature点集思想被提出，这就是著名的容积卡尔曼滤波（cubature Kalman filter， CKF）算法^［9］。该算法首先进行cubature点的计算与传播，通过状态方程迭代更新，计算观测量均值与估计协方差，最后用容积规则计算出卡尔曼增益，算法精度较先前算法有一定提高。

尽管传统CKF算法滤波精度有一定提高，但标准CKF算法在解决SLAM非线性问题时，随着观测特征的不断加入，导致计算量急剧增加，出现数值不稳定问题。对此，一系列自适应高阶容积卡尔曼滤波（HCKF）、CDKF等算法被提出，但它们基本都是结合协方差矩阵引入渐消因子来改进强跟踪能力，均存在自身的数值计算及滤波精度问题。结合文献［10-12］等所提的平方根容积卡尔曼滤波算法（SRCKF）思想，本文提出一种基于四元数的平方根容积卡尔曼滤波算法（QSRCKF）。该算法不仅降低了状态量维数及计算复杂度，而且平方根协方差因子的迭代更新可以确保算法的非负定性及对称性，新算法用于SINS/SLAM组合导航，可以降低载体的位置误差，提高载体姿态估计精度。

1 飞行器姿态四元数描述

利用SINS/SLAM组合导航对飞行器姿态进行估计，可以有效提高定位精度及强跟踪能力。假定当地坐标系为全局参考坐标o{x}_n{y}_n{z}_n（n）系，载体坐标系为o{x}_b{y}_b{z}_b（b）系。设在参考坐标n系下，载体系相对于n系的旋转次序为z\to x\to y，旋转角分别为偏航角\gamma、横滚角\alpha、俯仰姿态角分别为\beta，则由n\to b系的姿态旋转矩阵为：

{\mathit{C}}_n^b=\mathit{A}\mathit{B}\mathit{C}=\left[\begin{array}{ccc}{C}_{\mathrm{11}}& C_{\mathrm{12}}& -\mathrm{c}\mathrm{o}\mathrm{s}\alpha \mathrm{s}\mathrm{i}\mathrm{n}\beta \\ -\mathrm{c}\mathrm{o}\mathrm{s}\alpha \mathrm{s}\mathrm{i}\mathrm{n}\gamma & \mathrm{c}\mathrm{o}\mathrm{s}\beta \mathrm{c}\mathrm{o}\mathrm{s}\gamma & \mathrm{s}\mathrm{i}\mathrm{n}\alpha \\ C_{\mathrm{31}}& C_{\mathrm{32}}& \mathrm{c}\mathrm{o}\mathrm{s}\alpha \mathrm{c}\mathrm{o}\mathrm{s}\beta \end{array}\right],（1）

式中：

\begin{array}{l}\mathit{A}=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\beta & \mathrm{0}& -\mathrm{s}\mathrm{i}\mathrm{n}\beta \\ \mathrm{0}& \mathrm{1}& \mathrm{0}\\ \mathrm{s}\mathrm{i}\mathrm{n}\beta & \mathrm{0}& \mathrm{c}\mathrm{o}\mathrm{s}\beta \end{array}\right],\\ \mathit{B}=\left[\begin{array}{ccc}\mathrm{1}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{c}\mathrm{o}\mathrm{s}\alpha & \mathrm{s}\mathrm{i}\mathrm{n}\alpha \\ \mathrm{0}& -\mathrm{s}\mathrm{i}\mathrm{n}\alpha & \mathrm{c}\mathrm{o}\mathrm{s}\alpha \end{array}\right],\\ \mathit{C}=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\gamma & \mathrm{s}\mathrm{i}\mathrm{n}\gamma & \mathrm{0}\\ -\mathrm{s}\mathrm{i}\mathrm{n}\gamma & \mathrm{c}\mathrm{o}\mathrm{s}\gamma & \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right],\\ C_{\mathrm{11}}=\mathrm{c}\mathrm{o}\mathrm{s}\beta \mathrm{c}\mathrm{o}\mathrm{s}\gamma -\mathrm{s}\mathrm{i}\mathrm{n}\alpha \mathrm{s}\mathrm{i}\mathrm{n}\beta \mathrm{s}\mathrm{i}\mathrm{n}\gamma ,\\ C_{\mathrm{12}}=\mathrm{c}\mathrm{o}\mathrm{s}\beta \mathrm{s}\mathrm{i}\mathrm{n}\gamma +\mathrm{s}\mathrm{i}\mathrm{n}\alpha \mathrm{s}\mathrm{i}\mathrm{n}\beta \mathrm{c}\mathrm{o}\mathrm{s}\gamma ,\\ C_{\mathrm{31}}=\mathrm{s}\mathrm{i}\mathrm{n}\beta \mathrm{c}\mathrm{o}\mathrm{s}\gamma +\mathrm{s}\mathrm{i}\mathrm{n}\alpha \mathrm{c}\mathrm{o}\mathrm{s}\beta \mathrm{s}\mathrm{i}\mathrm{n}\gamma ,\\ C_{\mathrm{32}}=\mathrm{s}\mathrm{i}\mathrm{n}\beta \mathrm{s}\mathrm{i}\mathrm{n}\gamma -\mathrm{s}\mathrm{i}\mathrm{n}\alpha \mathrm{c}\mathrm{o}\mathrm{s}\beta \mathrm{c}\mathrm{o}\mathrm{s}\gamma \mathrm{。}\end{array}

定义载体姿态四元数为：

\mathit{q}={\left[q_{\mathrm{1}}{q}_{\mathrm{2}}{q}_{\mathrm{3}}{q}_{\mathrm{4}}\right]}^{\mathrm{T}}，（2）

则根据四元数定义，满足关系式：

{\mathit{q}}^{\mathrm{T}}\mathit{q}={‖\mathit{q}‖}^{\mathrm{2}}=q_{\mathrm{1}}^{\mathrm{2}}+q_{\mathrm{2}}^{\mathrm{2}}+q_{\mathrm{3}}^{\mathrm{2}}+q_{\mathrm{4}}^{\mathrm{2}}=\mathrm{1}。（3）

设向量\mathit{P}在n系及b系下的坐标分别为r^n及r^b，满足

r^b=C_n^b{r}^n,（4）

则通过坐标旋转关系及四元数旋转理论，可推算出由n\to b系四元数表示的载体姿态变换矩阵为：

{\mathit{C}}_n^b=\left[\begin{array}{ccc}{c}_{\mathrm{11}}& c_{\mathrm{12}}& c_{\mathrm{13}}\\ c_{\mathrm{21}}& c_{\mathrm{22}}& c_{\mathrm{23}}\\ c_{\mathrm{31}}& c_{\mathrm{32}}& c_{\mathrm{33}}\end{array}\right]，（5）

式中：

\begin{array}{l}{c}_{\mathrm{11}}=q_{\mathrm{1}}^{\mathrm{2}}+q_{\mathrm{2}}^{\mathrm{2}}-q_{\mathrm{3}}^{\mathrm{2}}-q_{\mathrm{4}}^{\mathrm{2}},c_{\mathrm{12}}=\mathrm{2}\left(q_{\mathrm{2}}{q}_{\mathrm{3}}+q_{\mathrm{1}}{q}_{\mathrm{4}}\right),\\ c_{\mathrm{13}}=\mathrm{2}\left(q_{\mathrm{2}}{q}_{\mathrm{4}}-q_{\mathrm{1}}{q}_{\mathrm{3}}\right),c_{\mathrm{21}}=\mathrm{2}\left(q_{\mathrm{2}}{q}_{\mathrm{3}}-q_{\mathrm{1}}{q}_{\mathrm{4}}\right),\\ c_{\mathrm{22}}=q_{\mathrm{1}}^{\mathrm{2}}+q_{\mathrm{3}}^{\mathrm{2}}-q_{\mathrm{2}}^{\mathrm{2}}-q_{\mathrm{4}}^{\mathrm{2}},c_{\mathrm{23}}=\mathrm{2}\left(q_{\mathrm{1}}{q}_{\mathrm{2}}+q_{\mathrm{3}}{q}_{\mathrm{4}}\right),\\ c_{\mathrm{31}}=\mathrm{2}\left(q_{\mathrm{1}}{q}_{\mathrm{3}}+q_{\mathrm{4}}{q}_{\mathrm{2}}\right),c_{\mathrm{32}}=\mathrm{2}\left(q_{\mathrm{3}}{q}_{\mathrm{4}}-q_{\mathrm{1}}{q}_{\mathrm{2}}\right),\\ c_{\mathrm{33}}=q_{\mathrm{1}}^{\mathrm{2}}+q_{\mathrm{4}}^{\mathrm{2}}-q_{\mathrm{2}}^{\mathrm{2}}-q_{\mathrm{3}}^{\mathrm{2}}\mathrm{。}\end{array}

根据欧拉角姿态矩阵与四元数姿态矩阵的转换关系，解算出载体姿态角为：

\begin{array}{l}\alpha =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{2}\left[\mathrm{2}\left(q_{\mathrm{1}}{q}_{\mathrm{2}}+q_{\mathrm{3}}{q}_{\mathrm{4}}\right){\alpha }_{\mathrm{1}}\right],\\ \beta =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left[\mathrm{2}\left(q_{\mathrm{2}}{q}_{\mathrm{4}}-q_{\mathrm{1}}{q}_{\mathrm{3}}\right){\beta }_{\mathrm{1}}\right],\\ \gamma =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{2}\left[\mathrm{2}\left(q_{\mathrm{2}}{q}_{\mathrm{3}}-q_{\mathrm{1}}{q}_{\mathrm{4}}\right)q_{\mathrm{1}}^{\mathrm{2}}+q_{\mathrm{2}}^{\mathrm{2}}-q_{\mathrm{3}}^{\mathrm{2}}-q_{\mathrm{4}}^{\mathrm{2}}\right],\end{array}（6）

式中：

\begin{array}{l}{\alpha }_{\mathrm{1}}=\sqrt[]{\mathrm{4}{\left(q_{\mathrm{2}}{q}_{\mathrm{4}}-q_{\mathrm{1}}{q}_{\mathrm{3}}\right)}^{\mathrm{2}}+{\left(q_{\mathrm{1}}^{\mathrm{2}}+q_{\mathrm{4}}^{\mathrm{2}}-q_{\mathrm{2}}^{\mathrm{2}}-q_{\mathrm{3}}^{\mathrm{2}}\right)}^{\mathrm{2}}},\\ {\beta }_{\mathrm{1}}=\sqrt[]{\mathrm{4}{\left(q_{\mathrm{1}}{q}_{\mathrm{2}}+q_{\mathrm{3}}{q}_{\mathrm{4}}\right)}^{\mathrm{2}}+{\left(q_{\mathrm{1}}^{\mathrm{2}}+q_{\mathrm{4}}^{\mathrm{2}}-q_{\mathrm{2}}^{\mathrm{2}}-q_{\mathrm{3}}^{\mathrm{2}}\right)}^{\mathrm{2}}}\mathrm{。}\end{array}

1.1 陀螺仪四元数模型

设k时刻b系下，载体相对于参考系的真实角速度为{\mathit{\omega }}_{nb}^b={\left[{\omega }_{nbx}^b{\omega }_{nby}^b{\omega }_{nbz}^b\right]}^{\mathrm{T}}。根据刚体转动定理，可求出载体姿态四元数微分方程为：

\dot{\mathit{q}}=\frac{\mathrm{1}}{\mathrm{2}}{\mathit{\Omega }}_{nb}^b\mathit{q}，（7）

式中：

{\mathit{\Omega }}_{nb}^b=\left[\begin{array}{l}\mathrm{0}-{\omega }_{nbx}^b-{\omega }_{nby}^b-{\omega }_{nbz}^b\\ {\omega }_{nbx}^b\mathrm{0}{\omega }_{nbz}^b-{\omega }_{nby}^b\\ {\omega }_{nby}^b-{\omega }_{nbz}^b\mathrm{0}{\omega }_{nbx}^b\\ {\omega }_{nbz}^b{\omega }_{nby}^b-{\omega }_{nbx}^b\mathrm{0}\end{array}\right]，

在理想条件下，四元数状态方程为：

{\mathit{q}}_k={\mathit{q}}_{k-\mathrm{1}}+\frac{△t}{\mathrm{2}}{\mathit{\Omega }}_{k|k-\mathrm{1}}{\mathit{q}}_{k-\mathrm{1}},（8）

式中：△t为角速度采样周期。

测得陀螺仪输出为{\omega }_{ib}^b，包括真实角速度量{\omega }_{nb}^b和陀螺仪随机漂移量{\omega }_{in}^b两部分，即

{\omega }_{ib}^b={\omega }_{nb}^b+{\omega }_{in}^b，

则陀螺仪模型表示为：

{\omega }_{nb}^b={\omega }_{ib}^b-{\omega }_{in}^b={\omega }_{ib}^b-C_n^b{\omega }_{in}^n,（9）

式中：{\omega }_{in}^n为陀螺漂移在参考系n系下的值。

采用CCD星敏感器的四元数输出作为陀螺仪姿态量测量，假设在相邻时刻内，角速度\omega的方向固定不变，由四元数表示的载体运动学方程的离散式为：

\begin{array}{l}{\mathit{q}}_{k+\mathrm{1}}=\mathit{\Omega }\left({\mathit{\omega }}_k\right){\mathit{q}}_k=\\ \left[\begin{array}{cc}\mathrm{c}\mathrm{o}\mathrm{s}(\frac{\mathrm{1}}{\mathrm{2}}‖{\mathit{\omega }}_k‖\mathrm{\Delta }t){\mathit{I}}_{\mathrm{3}\times \mathrm{3}}-{\mathit{\psi }}_k^{\times }& {\mathit{\psi }}_k\\ -{\mathit{\psi }}_k^{\mathrm{T}}& \mathrm{c}\mathrm{o}\mathrm{s}(\frac{\mathrm{1}}{\mathrm{2}}‖{\mathit{\omega }}_k‖\mathrm{\Delta }t){\mathit{I}}_{\mathrm{3}\times \mathrm{3}}\end{array}\right],\end{array}(10)

式中：{\mathit{\psi }}_k^{\times }为{\mathit{\psi }}_k的反对称矩阵，

{\mathit{\psi }}_k=\mathrm{s}\mathrm{i}\mathrm{n}(\frac{\mathrm{1}}{\mathrm{2}}‖{\mathit{\omega }}_k‖\mathrm{\Delta }t){\mathit{\omega }}_k/‖{\mathit{\omega }}_k‖。

对式（7）中的四元数微分方程展开：

\begin{array}{l}\dot{\mathit{q}}=\frac{\mathrm{1}}{\mathrm{2}}\mathit{\Omega }\left(\omega \right)\mathit{q}=\\ \frac{\mathrm{1}}{\mathrm{2}}\left[\begin{array}{l}\mathrm{0}-{\omega }_x-{\omega }_y-{\omega }_z\\ {\omega }_x\mathrm{0}{\omega }_z-{\omega }_y\\ {\omega }_y-{\omega }_z\mathrm{0}{\omega }_x\\ {\omega }_z{\omega }_y-{\omega }_x\mathrm{0}\end{array}\right]\mathit{q}\mathrm{。}\end{array}(11)

采用陀螺仪量测载体的角速度，状态模型为：

\left\{\begin{array}{l}{\mathit{\omega }}_g={\left[{\omega }_x{\omega }_y{\omega }_z\right]}^{\mathrm{T}}=\widehat{\mathit{\omega }}-\mathit{b}-{\mathit{\xi }}_a,\\ \dot{\mathit{b}}={\mathit{\xi }}_b,\end{array}\right.(12)

式中：{\mathit{\omega }}_g为真实角速度；\widehat{\mathit{\omega }}为实际陀螺仪输出值；\mathit{b}为陀螺仪漂移误差；{\mathit{\xi }}_a、{\mathit{\xi }}_b为系统高斯白噪声且分别满足

{\mathit{\xi }}_a~N\left(\mathrm{0},{\sigma }_a^{\mathrm{2}}\right)，{\mathit{\xi }}_b~N\left(\mathrm{0},{\sigma }_b^{\mathrm{2}}\right)。

1.2 星敏感器量测模型

星敏感器量测模型描述为：

z_k=\mathit{H}(q_k)\mathit{r}+v_k，(13)

式中：z_k为星敏感器的输出，\mathit{H}(q_k)为k时刻星敏感器量测姿态矩阵，r为星敏感器的参考向量，v_k为零均值高斯白噪声。

2 基于四元数的SRCKF‑SLAM算法

设SLAM模型中系统状态量及协方差矩阵分别为：

\mathit{x}=\left[x_v{x}_m\right]，\mathit{P}=\left[\begin{array}{l}{\mathit{P}}_{vv}{\mathit{P}}_{vm}\\ {\mathit{P}}_{vm}^{\mathrm{T}}{\mathit{P}}_{mm}\end{array}\right]，

式中：x_v、x_m分别为载体状态量及地图特征状态量，且各自包含x、y方向二维位置信息；{\mathit{P}}_{vv}、{\mathit{P}}_{vm}、{\mathit{P}}_{mm}分别为载体自相关协方差矩阵、载体与地图特征互协方差矩阵、地图特征自协方差矩阵。

在控制输入u_k下，将SLAM问题视为求取状态量x_v、x_m后验联合概率密度问题：

p\left(x_{v,k},x_m|z_k,u_k\right)=p\left(z_k|x_{v,k},x_m\right)\cdot \int p\left(x_{v,k}|x_{v,k-\mathrm{1}},u_k\right)\cdot

p\left(x_{v,k-\mathrm{1}},x_m|z_{k-\mathrm{1}},u_{k-\mathrm{1}}\right)\mathrm{d}{x}_{v,k-\mathrm{1}},(14)

式中：p\left(x_{v,k}|x_{v,k-\mathrm{1}},u_k\right)为运动模型，p\left(z_k|x_{v,k},x_m\right)为量测模型。

一般非线性运动模型可表示为：

\left\{\begin{array}{l}{x}_k=f\left(x_{k-\mathrm{1}}\right)+w_k,\\ z_k=h\left(x_k\right)+v_k,\end{array}\right.(15)

式中：f\left(\cdot \right)与h\left(\cdot \right)分别为非线性系统函数与观测函数；互不相关的系统高斯白噪声与观测高斯白噪声分别为

w_k~N\left(\mathrm{0},Q\right)，v_k~N\left(\mathrm{0},R\right)。

2.1 基于四元数的SRCKF算法

（1）状态参数初始化

\left\{\begin{array}{l}{\widehat{x}}_{\mathrm{0}}=E\left[x_{\mathrm{0}}\right],\\ P_{\mathrm{0}}=E\left[\left(x_{\mathrm{0}}-{\widehat{x}}_{\mathrm{0}}\right){\left(x_{\mathrm{0}}-{\widehat{x}}_{\mathrm{0}}\right)}^{\mathrm{T}}\right],\\ S_{\mathrm{0}}=\mathrm{C}\mathrm{h}\mathrm{o}\mathrm{l}\left\{E\left[\left(x_{\mathrm{0}}-{\widehat{x}}_{\mathrm{0}}\right){\left(x_{\mathrm{0}}-{\widehat{x}}_{\mathrm{0}}\right)}^{\mathrm{T}}\right]\right\},\end{array}\right.(16)

式中：\mathrm{C}\mathrm{h}\mathrm{o}\mathrm{l}表示矩阵\mathrm{C}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{e}\mathrm{s}\mathrm{k}\mathrm{y}分解。

（2）时间更新

设k-\mathrm{1}时刻联合后验概率

P\left(x_{k-\mathrm{1}}\right)=N\left({\widehat{x}}_{k-\mathrm{1}|k-\mathrm{1}},P_{k-\mathrm{1}|k-\mathrm{1}}\right)

已知，通过\mathrm{C}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{e}\mathrm{s}\mathrm{k}\mathrm{y}分解误差协方差

P_{k-\mathrm{1}|k-\mathrm{1}}=S_{k-\mathrm{1}|k-\mathrm{1}}{S}_{k-\mathrm{1}|k-\mathrm{1}}^{\mathrm{T}}，

式中：S_{k-\mathrm{1}|k-\mathrm{1}}为误差协方差子式。

①利用n维\mathrm{c}\mathrm{u}\mathrm{b}\mathrm{a}\mathrm{t}\mathrm{u}\mathrm{r}\mathrm{e}点生成\mathrm{2}n维\mathrm{c}\mathrm{u}\mathrm{b}\mathrm{a}\mathrm{t}\mathrm{u}\mathrm{r}\mathrm{e}点集：

x_{i,k-\mathrm{1}}=S_{k-\mathrm{1}}{\epsilon }_i+{\widehat{x}}_{k-\mathrm{1}}，(17)

式中：{\epsilon }_i为原容积点。

每个容积点对应的相等权值为：

{\tau }_i=\frac{\mathrm{1}}{\mathrm{2}n}，(i=\mathrm{1,2},\cdots ,\mathrm{2}n)。

②传播容积点并计算状态预测值：

x_{i,k|k-\mathrm{1}}^{\mathrm{*}}=f\left(x_{i,k-\mathrm{1}}\right),(18)

{\widehat{x}}_{k|k-\mathrm{1}}={\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{2}n}}{\tau }_i{x}_{i,k-\mathrm{1}}^{\mathrm{*}}。(19)

③计算状态误差协方差预测值：

\begin{array}{l}{P}_{k|k-\mathrm{1}}={\tau }_i{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{2}m}}\left(x_{k|k-\mathrm{1}}^{\mathrm{*}}{x}_{k|k-\mathrm{1}}^{\mathrm{*}\mathrm{T}}-\right.\\ \left.{\widehat{x}}_{k|k-\mathrm{1}}{\widehat{x}}_{k|k-\mathrm{1}}^{\mathrm{*}\mathrm{T}}+Q_{k-\mathrm{1}}\right),\end{array}(20)

改写Q_{k-\mathrm{1}}为\mathrm{C}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{e}\mathrm{s}\mathrm{k}\mathrm{y}分解式：

Q_{k-\mathrm{1}}=S_{Q,k-\mathrm{1}}{S}_{Q,k-\mathrm{1}}^{\mathrm{T}}。(21)

④状态误差协方差子式：

\left\{\begin{array}{l}{S}_{k|k-\mathrm{1}}=\mathit{q}\mathit{r}\left[{\chi }_{k|k-\mathrm{1}}{S}_{Q_{k-\mathrm{1}}}\right],\\ \begin{array}{l}{\chi }_{{}_{k|k-\mathrm{1}}}^{\mathrm{*}}=\frac{\mathrm{1}}{\sqrt[]{\mathrm{2}n}}[x_{k|k-\mathrm{1}}^{\mathrm{*},\mathrm{1}}-{\widehat{x}}_{k|k-\mathrm{1}},x_{k|k-\mathrm{1}}^{\mathrm{*},\mathrm{2}}-\\ {\widehat{x}}_{k|k-\mathrm{1}},\cdots ,x_{k|k-\mathrm{1}}^{\mathrm{*},\mathrm{2}n}-{\widehat{x}}_{k|k-\mathrm{1}}],\end{array}\end{array}\right.(22)

式中：\mathit{q}\mathit{r}为矩阵QR分解。

（3）量测更新

①计算容积点

x_{i,k|k-\mathrm{1}}=S_{k|k-\mathrm{1}}{\epsilon }_i+{\widehat{x}}_{k|k-\mathrm{1}},（i=\mathrm{1,2},\cdots ,\mathrm{2}n），(23)

②传播容积点

z_{i,k|k-\mathrm{1}}^{\mathrm{*}}=h\left(x_{i,k|k-\mathrm{1}}\right)，(24)

③计算k时刻观测预测值：

{\widehat{z}}_{k|k-\mathrm{1}}=\frac{\mathrm{1}}{\mathrm{2}n}{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{2}n}}{z}_{k|k-\mathrm{1}}^{\mathrm{*}},(25)

④计算自相关协方差

P_{zz,k|k-\mathrm{1}}={\tau }_i{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{2}n}}(z_{i,k|k-\mathrm{1}}{z}_{i,k|k-\mathrm{1}}^{\mathrm{T}}-{\widehat{z}}_{i,k|k-\mathrm{1}}{\widehat{z}}_{i,k|k-\mathrm{1}}^{\mathrm{T}}+R_k），(26)

改写R_{k-\mathrm{1}}为\mathrm{C}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{e}\mathrm{s}\mathrm{k}\mathrm{y}分解式：

R_{k-\mathrm{1}}=S_{R,k-\mathrm{1}}{S}_{R,k-\mathrm{1}}^{\mathrm{T}},(27)

误差协方差平方根子式

\left\{\begin{array}{l}{S}_{zz,k|k-\mathrm{1}}=qr\left[{\eta }_{k|k-\mathrm{1}}{S}_{R_k}\right],\\ \begin{array}{l}{\eta }_{k|k-\mathrm{1}}=\frac{\mathrm{1}}{\sqrt[]{\mathrm{2}n}}{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{2}n}}(z_{i,k|k-\mathrm{1}}^{\mathrm{*},\mathrm{1}}-{\widehat{z}}_{k|k-\mathrm{1}},z_{i,k|k-\mathrm{1}}^{\mathrm{*},\mathrm{2}}-\\ {\widehat{z}}_{k|k-\mathrm{1}},\cdots ,z_{i,k|k-\mathrm{1}}^{\mathrm{*},\mathrm{2}n}-{\widehat{z}}_{k|k-\mathrm{1}}),\end{array}\end{array}\right.(28)

⑤估计（权值）互协方差平方根子式有：

\left\{\begin{array}{l}\begin{array}{l}{\chi }_{k|k-\mathrm{1}}=\frac{\mathrm{1}}{\sqrt[]{\mathrm{2}n}}[x_{k|k-\mathrm{1}}^{\mathrm{1}}-{\widehat{x}}_{k|k-\mathrm{1}},x_{k|k-\mathrm{1}}^{\mathrm{2}}-\\ {\widehat{x}}_{k|k-\mathrm{1}},\cdots ,x_{k|k-\mathrm{1}}^{\mathrm{2}n}-{\widehat{x}}_{k|k-\mathrm{1}}],\end{array}\\ P_{xz,k|k-\mathrm{1}}={\chi }_{k|k-\mathrm{1}}{\eta }_{k|k-\mathrm{1}}^{\mathrm{T}}\mathrm{。}\end{array}\right.(29)

卡尔曼增益为

K_k=\left(P_{xz,k|k-\mathrm{1}}/S_{zz,k|k-\mathrm{1}}^{\mathrm{T}}\right)S_{zz,k|k-\mathrm{1}}。(30)

相关状态误差协方差平方根子式为

S_{k|k}=qr\left[{\chi }_{k|k-\mathrm{1}}-K_k{\eta }_{k|k-\mathrm{1}},k_k{S}_{R,k}\right]。(31)

四元素SRCKF选取姿态误差\delta x_{i,k-\mathrm{1}}^q及陀螺漂移误差\delta x_{i,k-\mathrm{1}}^b为联合状态：

\delta x_{i,k-\mathrm{1}}={\left[\delta x_{i,k-\mathrm{1}}^q,\delta x_{i,k-\mathrm{1}}^b\right]}^{\mathrm{T}}=S_{k-\mathrm{1}}{\epsilon }_i,(32)

式中：i=\mathrm{1,2},\cdots ,\mathrm{2}n。

四元数SRCKF状态容积点分别为：

\left\{\begin{array}{l}{x}_{i,k-\mathrm{1}}^q=\delta {\widehat{q}}_{i,k-\mathrm{1}}\otimes {\widehat{q}}_{k-\mathrm{1}},\\ x_{i,k-\mathrm{1}}^b={\widehat{x}}_{k-\mathrm{1}}^b+\delta {\widehat{x}}_{i,k-\mathrm{1}}^b\mathrm{。}\end{array}\right.(33)

定义误差四元数为

\delta x_{i,k-\mathrm{1}}^q={\widehat{q}}_{k|k-\mathrm{1}}\otimes {\left(x_{i,k-\mathrm{1}}^q\right)}^{-\mathrm{1}}。(34)

2.2 基于四元数SRCKF‑SLAM算法

（1）时间更新

\begin{array}{l}{x}_{i,k|k-\mathrm{1}}=f\left(x_{i,k-\mathrm{1}}\right),\widehat{b}={\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{2}n}}{\tau }_i{x}_{i,k|k-\mathrm{1}}^b,\\ S_{k|k-\mathrm{1}}^q=qr\left[{\chi }_{q,k|k-\mathrm{1}}^{\mathrm{*}}{S}_{Q_{k-\mathrm{1}}}^q\right],\\ S_{k|k-\mathrm{1}}^b=qr\left[{\chi }_{b,k|k-\mathrm{1}}^{\mathrm{*}}{S}_{Q_{k-\mathrm{1}}}^b\right],\\ S_{k|k-\mathrm{1}}^{q,b}=qr{\left[\begin{array}{l}{\tau }_i\delta x_{k|k-\mathrm{1}}^{q,\mathrm{1}}{\left(x_{k|k-\mathrm{1}}^{b,\mathrm{1}}-{\widehat{b}}_{k|k-\mathrm{1}}\right)}^{\mathrm{T}}\\ {\tau }_i\delta x_{k|k-\mathrm{1}}^{q,\mathrm{2}}{\left(x_{k|k-\mathrm{1}}^{b,\mathrm{2}}-{\widehat{b}}_{k|k-\mathrm{1}}\right)}^{\mathrm{T}}\\ ⋮\\ {\tau }_i\delta x_{k|k-\mathrm{1}}^{q,\mathrm{2}n}{\left(x_{k|k-\mathrm{1}}^{b,\mathrm{2}n}-{\widehat{b}}_{k|k-\mathrm{1}}\right)}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}},\\ S_{k|k-\mathrm{1}}=\left[\begin{array}{l}{S}_{k|k-\mathrm{1}}^q{S}_{k|k-\mathrm{1}}^{q,b}\\ {\left(S_{k|k-\mathrm{1}}^{q,b}\right)}^{\mathrm{T}}{S}_{k|k-\mathrm{1}}^b\end{array}\right],\end{array}(35)

式中：\widehat{b}为陀螺漂移部分预测值，S_{k|k-\mathrm{1}}^q、S_{k|k-\mathrm{1}}^b分别为位姿估计及陀螺漂移误差平方根协方差子式，且

\left\{\begin{array}{l}{\chi }_{q,k|k-\mathrm{1}}^{\mathrm{*}}=\frac{\mathrm{1}}{\mathrm{2}n}\left[\delta x_{k|k-\mathrm{1}}^{q,\mathrm{1}},\delta x_{k|k-\mathrm{1}}^{q,\mathrm{2}},\cdots ,\delta x_{k|k-\mathrm{1}}^{q,\mathrm{2}n}\right],\\ \begin{array}{l}{\chi }_{b,k|k-\mathrm{1}}^{\mathrm{*}}=\frac{\mathrm{1}}{\mathrm{2}n}[x_{k|k-\mathrm{1}}^{b,\mathrm{1}}-{\widehat{b}}_{k|k-\mathrm{1}},x_{k|k-\mathrm{1}}^{b,\mathrm{2}}-\\ {\widehat{b}}_{k|k-\mathrm{1}},\cdots ,x_{k|k-\mathrm{1}}^{b,\mathrm{2}\mathrm{n}}-{\widehat{b}}_{k|k-\mathrm{1}}]\mathrm{。}\end{array}\end{array}\right.(36)

（2）量测更新

①更新四元数SRCKF状态容积点：

{\xi }_{i,k|k-\mathrm{1}}^q=\delta z_{i,k|k-\mathrm{1}}^q\otimes {\widehat{q}}_{k|k-\mathrm{1}}。(37)

②量测更新及误差自协方差子式及互协方差更新：

z_{i,k-\mathrm{1}}^q=\delta {\widehat{q}}_{i,k-\mathrm{1}}\otimes {\xi }_{i,k|k-\mathrm{1}}^q\otimes {\widehat{q}}_{k-\mathrm{1}},

\delta z_{i,k}=S_{k|k-\mathrm{1}}{\epsilon }_i={\left[\delta z_{i,k-\mathrm{1}}^q,\delta z_{i,k-\mathrm{1}}^b\right]}^{\mathrm{T}},

S_{zz,k}=qr\left[{\eta }_{z,k}{S}_{R_{k-\mathrm{1}}}^{\eta }\right],

P_{xz,k}={\left[\begin{array}{l}{\tau }_i{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{2}n}}\delta x_{k|k-\mathrm{1}}^q{\left(\delta z_{k|k-\mathrm{1}}^q\right)}^{\mathrm{T}}\\ {\tau }_i{\displaystyle \sum _{i=\mathrm{1}}^{\mathrm{2}n}}\left(x_{i,k|k-\mathrm{1}}^b-{\widehat{b}}_{k|k-\mathrm{1}}\right){\left(\delta z_{i,k|k-\mathrm{1}}^q\right)}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}},(38)

\left\{\begin{array}{l}\delta z_{i,k-\mathrm{1}}^q={\widehat{z}}_{k|k-\mathrm{1}}\otimes (\delta z_{i,k-\mathrm{1}}^q{)}^{-\mathrm{1}},\\ {\eta }_{z,k}=\frac{\mathrm{1}}{\mathrm{2}n}\left[\delta z_k^{q,\mathrm{1}},\delta z_k^{q,\mathrm{2}},\cdots ,\delta z_k^{q,\mathrm{2}n}\right]\mathrm{。}\end{array}\right.(39)

卡尔曼增益：

K_k=\left(P_{xz}/S_{zz,k}^{\mathrm{T}}\right)\cdot \frac{\mathrm{1}}{S_{zz,k}}，(40)

状态误差量：

\delta {\widehat{x}}_k=K_k\delta {\widehat{z}}_{k|k-\mathrm{1}}，(41)

式中：

\delta {\widehat{x}}_k={\left[\delta {\widehat{q}}_k^{\mathrm{T}}\delta {\widehat{b}}_k^{\mathrm{T}}\right]}^{\mathrm{T}}，

\delta {\widehat{z}}_{k|k-\mathrm{1}}=z_k\otimes {\left({\widehat{z}}_{k|k-\mathrm{1}}\right)}^{-\mathrm{1}}，\delta {\widehat{q}}_k=\frac{\delta {\widehat{q}}_k}{{‖\delta {\widehat{q}}_k‖}_{\mathrm{2}}}。

③陀螺漂移更新

\left\{\begin{array}{l}{\widehat{q}}_k=\delta {\widehat{q}}_k\otimes {\widehat{q}}_{k|k-\mathrm{1}},\\ {\widehat{b}}_k={\widehat{b}}_{k|k-\mathrm{1}}+\delta {\widehat{b}}_k,\end{array}\right.(42)

状态量误差协方差矩阵为：

{\mathit{S}}_k=\mathrm{c}\mathrm{h}\mathrm{o}\mathrm{l}\left({\mathit{S}}_k{\mathit{S}}_{k|k-\mathrm{1}}^{\mathrm{T}},{\mathit{U}}_k{\mathit{U}}_k^{\mathrm{T}},-\mathrm{1}\right)，(43)

式中：{\mathit{U}}_k={\mathit{K}}_k{\mathit{S}}_{zz,k}。

3 SRCDKF、SRUKF、SRCKF算法复杂度分析

设状态转移矩阵为\mathit{F}\in {\mathbf{R}}_{n\times m}，观测矩阵为\mathit{H}\in {\mathbf{R}}_{m\times l}，矩阵求逆、Cholesky分解、QR分解的flops计算分别为n^{\mathrm{3}}、n^{\mathrm{3}}/\mathrm{3}、\mathrm{2}m{n}^{\mathrm{2}}。由文献［11-12］以及CDKF、UKF、CKF算法复杂度：

\left\{\begin{array}{l}\begin{array}{l}O{\left(n^{\mathrm{3}}\right)}_{\mathrm{C}\mathrm{K}\mathrm{D}\mathrm{F}}=\frac{\mathrm{26}}{\mathrm{3}}{n}^{\mathrm{3}}+\mathrm{13}{n}^{\mathrm{2}}+\mathrm{8}{n}^{\mathrm{2}}l+\mathrm{2}n+\\ \mathrm{8}n{l}^{\mathrm{2}}+\mathrm{5}nl+l^{\mathrm{3}}+\mathrm{2}{l}^{\mathrm{2}}+\mathrm{3}l,\end{array}\\ \begin{array}{l}O{\left(n^{\mathrm{3}}\right)}_{\mathrm{U}\mathrm{K}\mathrm{F}}=\frac{\mathrm{26}}{\mathrm{3}}{n}^{\mathrm{3}}+\mathrm{15}{n}^{\mathrm{2}}+\mathrm{10}{n}^{\mathrm{2}}l+\mathrm{5}n+\\ \mathrm{8}n{l}^{\mathrm{2}}+\mathrm{6}nl+l^{\mathrm{3}}+\mathrm{3}{l}^{\mathrm{2}}+\mathrm{4}l,\end{array}\\ \begin{array}{l}O{\left(n^{\mathrm{3}}\right)}_{\mathrm{C}\mathrm{K}\mathrm{F}}=\frac{\mathrm{20}}{\mathrm{3}}{n}^{\mathrm{3}}+\mathrm{10}{n}^{\mathrm{2}}+\mathrm{10}{n}^{\mathrm{2}}l+\mathrm{8}n{l}^{\mathrm{2}}+\\ \mathrm{2}nl+l^{\mathrm{3}}+\mathrm{3}{l}^{\mathrm{2}}+l,\end{array}\end{array}\right.(44)

推算SRCDKF、SRUKF、SRCKF复杂度，结果如表1所示。

对上述算法的复杂度进行整理得到：

\left\{\begin{array}{l}\begin{array}{l}O{\left(n^{\mathrm{3}}\right)}_{\mathrm{S}\mathrm{R}\mathrm{C}\mathrm{D}\mathrm{K}\mathrm{F}}=\mathrm{8}{n}^{\mathrm{3}}+\mathrm{8}{n}^{\mathrm{2}}+\mathrm{6}{n}^{\mathrm{2}}l+n+\\ \mathrm{8}n{l}^{\mathrm{2}}+nl+l^{\mathrm{3}}+l,\end{array}\\ \begin{array}{l}O{\left(n^{\mathrm{3}}\right)}_{\mathrm{S}\mathrm{R}\mathrm{U}\mathrm{K}\mathrm{F}}=\mathrm{8}{n}^{\mathrm{3}}+\mathrm{10}{n}^{\mathrm{2}}+\mathrm{12}{n}^{\mathrm{2}}l+\\ \mathrm{3}n+\mathrm{6}n{l}^{\mathrm{2}}+\mathrm{3}nl+l^{\mathrm{3}}+\mathrm{2}l,\end{array}\\ \begin{array}{l}O{\left(n^{\mathrm{3}}\right)}_{\mathrm{S}\mathrm{R}\mathrm{C}\mathrm{K}\mathrm{F}}=\mathrm{6}{n}^{\mathrm{3}}+\mathrm{6}{n}^{\mathrm{2}}+\mathrm{8}{n}^{\mathrm{2}}l+\\ \mathrm{10}n{l}^{\mathrm{2}}+nl+l^{\mathrm{3}}+l\mathrm{。}\end{array}\end{array}\right.（45）

由式（45）可知，SRUKF、SRCDKF、SRCKF三种算法复杂度f\propto \left(O\left(n^{\mathrm{3}}\right)+O\left(l^{\mathrm{3}}\right)\right)级，且f_{\mathrm{S}\mathrm{R}\mathrm{U}\mathrm{K}\mathrm{F}}>f_{\mathrm{S}\mathrm{R}\mathrm{C}\mathrm{D}\mathrm{K}\mathrm{F}}>f_{\mathrm{S}\mathrm{R}\mathrm{C}\mathrm{K}\mathrm{F}}，复杂度依次降低。随着状态量维数的增大，SRCKF滤波算法的优势越来越明显。

4 仿真实验与分析

4.1 特征观测实验

为了验证本文提出的SLAM算法的有效性和可行性，进行了Matlab仿真实验。模拟环境是在悉尼大学学者Tim Bailey发布的开源SLAM模拟器中建立的。实验环境位于100 m×100 m的室外环境区域，包括人工设置的载体实际运行路径和20个固定路标。载体从坐标（60，10）沿着模拟和实际确定的轨迹顺时针移动。具体实验环境如图1所示。

图2显示了SLAM算法在机器人位置估计中对20个特征目标的实际观测结果。图1和图2中绿线及蓝线表示机器人的实际导航路径，红线表示SLAM算法下机器人的估计跟踪路径，*点表示模拟区域中存在的20个实际特征目标对象。实验结果表明，当机器人沿着指定的路径移动时，基于SLAM算法中的数据融合和观测，最终观测到导航路径下的第3、6、7、11、12、15、16、17、18、19和20共11个特征点。每次观测都要进行多次判断，最终确定有效观测点的位置。

4.2 三种滤波算法对姿态角估计精度比较

以近地卫星为例，星敏感器垂直安装，建立SINS/SLAM组合导航与CCD姿态估计系统平台，在Matlab与C++软件进行测试。假定卫星轨道高度800 km，倾角82°，飞行器初始位置为东经120°、北纬45°，飞行轨迹及跟踪估计如图3所示。初始姿态角误差及陀螺初始漂移为零，陀螺初始漂移误差方差矩阵为\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}{\left(\mathrm{0.2,0.2,0.2}\right)}_{\mathrm{3}\times \mathrm{3}}，初始姿态角误差方差矩阵为\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}{\left(\mathrm{18,18,18}\right)}_{\mathrm{3}\times \mathrm{3}}，初始过程噪声为\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}(\mathrm{0}.{\mathrm{02}}^{\mathrm{2}}{\mathit{I}}_{\mathrm{3}\times \mathrm{3}},\mathrm{0}.{\mathrm{05}}^{\mathrm{2}}{\mathit{I}}_{\mathrm{3}\times \mathrm{3}})，初始量测噪声方差矩阵为\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}{\left[\mathrm{20,20,20}\right]}_{\mathrm{3}\times \mathrm{3}}，仿真时间为600 s，跟踪估计结果如图3所示，其他参数见表2。

为了验证所提模型估计精度的优越性，试验对比了四元数SRUKF-SLAM、四元数SRCDKF-SLAM算法，估计横滚姿态角误差、俯仰姿态角误差及偏航姿态角误差大小，仿真结果如图4—图6所示。

从图4—图6可以看出，在给定初始条件下，对于横滚姿态角、俯仰姿态角及偏航姿态角的估计，基于四元数SRCDKF-SLAM的估计误差最大，基于四元数SRUKF-SLAM算法的估计误差较前者有明显减小，基于四元数SRCKF-SLAM算法较以上两种算法最小，导航解算时的计算复杂度比其他两种算法小，因此误差最小，且能够较快地收敛于零，数值稳定性最好。

3种滤波算法对姿态角估计误差均值统计见表2。可以看出，四元数SRCDKF-SLAM算法、四元数SRUKF-SLAM算法、四元数SRCKF-SLAM算法的估计误差依次降低，且新算法单次迭代时间最短，耗时性能最优，与仿真图形一致。

4.3 大初始误差角条件下实验仿真与分析

四元数SRCDKF、四元数SRUKF算法对姿态角估计误差较大，导致3种滤波仿真图形纵坐标比例较大。为了体现四元数SRCKF算法对SLAM非线性模型大失准姿态角估计的鲁棒性与滤波精度，此组实验设定初始横滚角姿态角误差、俯仰姿态角误差、偏航姿态角误差分别为30°、30°、60°，陀螺仪初始条件与上节一致，仿真时间500 s，仿真结果如图7所示。

由图7可以看出，在给定大误差初始姿态角下，基于四元数SRCKF-SLAM算法对姿态角的估计误差能够在仿真时间的前50 s内迅速减小到{\mathrm{0}}^{″}左右，收敛速度快，且随着仿真时间的进行，滤波始终保持稳定，表明该算法对于大失准姿态角滤波估计具有较强的鲁棒性及稳定性。

4.4 3种滤波算法下无人机位置误差的估计

在上述实验环境下，对SRCKF-SLAM、SRUKF-SLAM和SRCDF-SLAM进行了50次独立的重复模拟实验，并对载波路径方向上的估计误差进行了分析和比较。结果如图8—图10所示。

从图8中可以看出，SRCKF-SLAM算法在植保无人机导航轨迹x方向上的误差范围约为[-\mathrm{0.1,0.6}] m，均值误差约为0.25 m。SRUKF-SLAM和SRCDF-SLAM算法的误差分别约为[-\mathrm{0.1,2.4}] m和[-\mathrm{0.1,2.4}] m，但二者的均值误差都比较大。可以看出，前者在无人机x方向上的平均估计误差最小，误差波动范围最小，稳定性最好。

从图9中可以看出，SRCKF-SLAM算法在植保无人机导航轨迹y方向上的误差范围约为[-\mathrm{0.2,0.5}] m，误差均值约为0.05 m。SRUKF-SLAM和SRCDF-SLAM算法的误差分别约为[-\mathrm{0.1,3.0}] m和[-\mathrm{2.0,0.9}] m，二者的误差均值较大。可以看出，前者在无人机y方向上的平均位置估计误差最小，误差波动范围最小，稳定性最好。基于SRCDF-SLAM算法的误差最大，滤波波动幅度也最大，其次是SRUKF-SLAM算法的误差。

从图10中可以看出，SRCKF-SLAM算法对植保无人机z方向导航轨迹的误差范围约为［-0.48，0.25］ m，误差均值约为0.05 m。SRUKF-SLAM和SRCDF-SLAM算法的误差分别约为［0，2.5］ m和［-1.5，2.5］ m，但二者的误差均值较大。由此可以看出，前者在无人机的z方向上具有最小的平均位置估计误差，并且在整个模拟时间内其误差位置都在小范围内，这表明植保无人机可以在基本位置进行恒定高度喷洒作业，并且具有很高的稳定性。该滤波算法具有较强的一致收敛性。基于SRCDF-SLAM算法的误差最大，滤波波动幅度也最大，其次是SRUKF-SLAM算法的误差。

对植保无人机导航轨迹的位置误差估计实验表明，SRCKF-SLAM算法在3个方向上的平均滤波误差最小、误差波动范围最小、收敛性最好。

5 结论

针对传统四元数SRUKF算法、传统四元数SRCDKF算法中导航解算复杂，计算量大导致滤波不稳定且精度差等问题，提出一种基于四元数的SRCKF算法，并应用于SLAM的非线性模型，对给定同等条件下的3个初始姿态角误差下进行了仿真实验，结果表明新算法对载体横滚角误差估计均值、俯仰角估计误差均值、偏航角估计误差均值分别为{\mathrm{2}}^{″}、{\mathrm{3}}^{″}、{\mathrm{1}}^{″}，算法精度较四元数SRUKF-SLAM算法提高了30%左右。另外，在给定大失准角误差下，进行了基于四元数SRCKF-SLAM的对载体姿态角的滤波实验，结果表明，该算法滤波误差能够在仿真时间的前50 s内迅速减小到{\mathrm{0}}^{″}左右，收敛速度快，且随着仿真时间的进行，滤波始终保持稳定，表现出了很强的收敛性与稳定性。

估计了SRUKF-SLAM、SRCDKF-SLAM、SRUKF-SLAM等3种滤波算法下无人机位置信息，SRCKF-SLAM算法在植保无人机导航轨迹x\mathrm{、}y\mathrm{、}z三个方向上的误差范围约为[-\mathrm{0.1,0.6}] m、[-\mathrm{0.2,0.5}] m、[-\mathrm{0.48,0.25}] m，且误差均值均较小，实验结果再次表明了所提算法在植保无人机导航估计中滤波精度较其他两种算法高，滤波速度快，稳定性强的优点。

参考文献

[1] ZHAO Yuxin， YANG Shuo， JIA Renfeng， et al. The statistical observation localized equivalent-weights particle filter in a simple nonlinear model［J］. Acta Oceanologica Sinica， 2022， 41（2）： 80-90.

[2] CAO H Q， NGUYEN H X， TRAN T N C， et al. A robot calibration method using a neural network based on a butterfly and flower pollination algorithm［J］. IEEE Transactions on Industrial Electronics， 2022， 69（4）： 3865-3875.

[3] ZHANG He， QIN Weiwei， ZHOU Cheng， et al. Attitude determination algorithm for micro-satellite based on high-order UKF using information fusion［J］. Chinese Journal of Space Science， 2020， 40（6）： 1091-1101.

[4] 闵艳玲， 熊智， 邢丽， 等. 基于对偶四元数的惯性/卫星/天文组合导航系统改进联邦滤波方法［J］. 兵工学报， 2018， 39（2）： 315-324.

[5] MIN Yanling， XIONG Zhi， XING Li， et al. An improved SINS/GNSS/CNS federal filter based on dual quaternions［J］. Acta Armamentarii， 2018， 39（2）： 315-324.

[6] 周成宝. 飞行器姿态非线性控制方法研究［D］. 哈尔滨： 哈尔滨工业大学， 2016.

[7] ZHOU Chengbao. Nonlinear control methods for flight vehicle attitude［D］. Harbin： Harbin Institute of Technology， 2016．

[8] LUO Xin， ZHOU Mengchu， LI Shuai， et al. Non-negativity constrained missing data estimation for high-dimensional and sparse matrices from industrial applications［J］. IEEE Transactions on Cybernetics， 2020， 50（5）： 1844-1855.

[9] JIA R. Attitude estimation algorithm for low cost MEMS based on quaternion EKF［J］. Chinese Journal of Sensors and Acthators， 2014， 27（1）： 90-95.

[10] WANG D， TAN K， DONG Y， et al. Estimating the position and orientation of a mobile robot using neural network framework based on combined square-root cubature Kalman filter and simultaneous localization and mapping［J］. Advances in Production Engineering &Amp Management， 2020， 15（1）： 31-43.

[11] WANG Dandan， TAN Kaituo， LI Zhengbin， et al. Research on landmarks of SLAM based on square root cubature kalman filter［J］. Journal of Physics： Conference Series， 2018， 1069（1）： 012154.

[12] WANG Dandan， TAN Kaituo， LI Hongjie. Research on feature extraction method based on simultaneous localization and mapping［C］//2018 37th Chinese Control Conference （CCC）， Wuhan， 2018： 3720-3724.

[13] XIONG Yufeng， ZHANG Yu， GUO Xiaoting， et al. Seamless global positioning system/inertial navigation system navigation method based on square-root cubature Kalman filter and random forest regression［J］. Review of Scientific Instruments， 2019， 90（1）： 015101.

[14] 王碧霞， 李银伢， 戚国庆， 等. 基于SR-UKF的纯方位多移动机器人协同定位算法［J］. 南京理工大学学报， 2015， 39（4）： 440-446.

[15] WANG Bixia， LI Yinya， QI Guoqing， et al. Bearings-only cooperative localization algorithm of multi-mobile robots based on square-root unscented Kalman filter［J］. Journal of Nanjing University of Science and Technology， 2015， 39（4）： 440-446.

基金资助

国家自然科学基金项目(61201409)

安徽省教育厅高校科研重点项目(2023AH051547)

安徽省教育厅高校科研重点项目(2023AH051559)

安徽省中青年优秀教师培育项目(YQYB2023031)

河南省高等学校重点科研项目(21A590001)

淮南市科技局指导性科技计划项目(126)

校培育基金重点项目(YPY2020001)

校级质量工程课程思政示范课程(2023hskc06)

安徽省质量工程项目(2023jyxm0796)