[发明专利]一种基于自适应EKF的无人侦察机同步定位与构图方法

摘要

本发明属于无人机技术领域，具体涉及一种基于自适应EKF的无人侦察机同步定位与构图方法。其建立无人侦察机同步定位与地图构建系统的地图模型、坐标系统、特征模型、无人侦察机的运动模型以及传感器测量模型。利用EKF实现无人侦察机的同步定位与地图构建方法框架的设计。并针对无人侦察机运动模型、观测模型构建不准确、以及非线性模型线性化导致的模型不匹配问题研究。提出将模型构建不准确以及线性化误差导致的不确定性建模为具有时变均值及协方差矩阵的系统过程噪声和观测噪声，设计基于带时变噪声估值器的自适应EKF的同步定位与构图方法，实时估计系统过程噪声和观测的统计特性并对其进行修正，从而降低模型不确定性，提高估计进度。

主权项

一种基于自适应EKF的无人侦察机同步定位与构图方法，其特征在于，该方法包括如下步骤：步骤S1：对无人侦察机SLAM系统建模；1.1地图模型采用特征地图来构建环境地图模型，将环境中的目标用几何原型来描述；1.2坐标系统由于无人侦察机所携带的传感器与环境特征之间的感知行为发生在传感器坐标系中，速度的测量发生在无人侦察机运动坐标系中，而最终环境地图的构建需要表示在全局地图中，所以建立全局坐标系，无人侦察机运动坐标系及传感器坐标系，并表明他们之间的关系，定义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)，定义特征的运动学模型为：$X_{f_i}\left(k\right)=X_{f_i}(k-1),i=1...n---\left(1\right)$其中，k为离散时间变量；特征在传感器坐标系中一般以极坐标的形式表示：$X_{f_i}\left(k\right)={\left[\begin{array}{c}{\mathrm{\ρ}}_i\\ {\mathrm{\θ}}_i\end{array}\right]}_{\left(k\right)}$其中，表示第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)${\left[\begin{array}{c}x\\ y\\ z\\ \mathrm{\ψ}\\ u\\ v\\ w\\ r\end{array}\right]}_{\left(k\right)}={\left[\begin{array}{c}x+(uT+n_u\frac{T^2}{2})cos\mathrm{\ψ}-(vT+n_v\frac{T^2}{2})sin\mathrm{\ψ}\\ y+(uT+n_u\frac{T^2}{2})sin\mathrm{\ψ}+(vT+n_v\frac{T^2}{2})cos\mathrm{\ψ}\\ z+wT+n_w\frac{T^2}{2}\\ \mathrm{\ψ}+rT+n_r\frac{T^2}{2}\\ u+n_{u}T\\ v+n_{v}T\\ w+n_{w}T\\ r+n_{r}T\end{array}\right]}_{(k-1)}$其中，T为采样时间；1.5传感器测量模型(1)速度测量模型速度测量传感器可提供X、Y、Z三个方向的速度，测量模型为：Z_V＝H_VX+s_V   (4)其中，Z_V为速度的测量值；测量矩阵H_V为：H_V＝[0_3×4 I_3×3 0_3×1 0_3×2n]其中，O为全零矩阵，I为单位矩阵；速度测量噪声s_V的协方差矩阵为：$R_V=\left[\begin{array}{ccc}{\mathrm{\σ}}_{Vu}^2& 0& 0\\ 0& {\mathrm{\σ}}_{Vv}^2& 0\\ 0& 0& {\mathrm{\σ}}_{Vw}^2\end{array}\right]$其中，为速度u的方差，为速度v的方差，为速度w的方差；(2)高度测量模型通过高度测量传感器可以获得无人侦察机的高度，测量模型为：Z_H＝H_HX+s_H   (5)其中，Z_H为高度的测量值；测量矩阵H_H为：H_H＝[0 0 1 0 0 0 0 0 0_1×2n]高度测量噪声的s_H协方差矩阵表示为：$R_H={\mathrm{\σ}}_H^2$(3)艏向测量模型通过艏向测量传感器可以获得无人侦察机的艏向，测量模型为：Z_C＝H_CX+s_C   (6)其中，Z_C为艏向的测量值；测量矩阵H_C为：H_C＝[0 0 0 1 0 0 0 0 0_1×2n]艏向测量噪声s_C的协方差为：$R_C={\mathrm{\σ}}_C^2$(4)环境测量模型通过环境感知传感器可以获得环境中特征相对于无人侦查机的距离和方位，测量模型为：$Z_i^A={\left[\begin{array}{cc}{\mathrm{\ρ}}_i^A& {\mathrm{\θ}}_i^A\end{array}\right]}^T=h\left(X\right(k),s_i)---\left(7\right)$其中，s_i为特征测量噪声；$h\left(X\right(k),s_i)=\left[\begin{array}{c}\sqrt{{({\mathrm{\ρ}}_i{\mathrm{cos\θ}}_i-x)}^2+{({\mathrm{\ρ}}_i{\mathrm{sin\θ}}_i-y)}^2}\\ arctan\left(\frac{{\mathrm{\ρ}}_i{\mathrm{sin\θ}}_i-y}{{\mathrm{\ρ}}_i{\mathrm{cos\θ}}_i-x}\right)-\mathrm{\ψ}\end{array}\right]+s_i$测量矩阵为：其中，和是点特征在运动坐标系下的参数表示，ρ_i和θ_i是点特征在全局坐标系下的参数表示；步骤S2：基于步骤S1所建立的无人侦察机SLAM系统模型，执行自适应EKF的无人侦察机同步定位与构图操作；其中，建立无人侦察机SLAM系统的状态方程和观测方程如下所示，被估计状态以及观测变量与状态向量的关系呈现非线性：X(k)＝f(X(k‑1),m(k))                                 (8)Z(k)＝h(X(k),n(k))式中，X(k)是无人侦察机SLAM系统的状态向量；Z(k)是系统的观测向量；m(k)是系统的过程噪声序列；n(k)是系统的观测噪声序列；f()和h()表示系统的非线性状态方程和非线性观测方程；针对上式所示的非线性SLAM系统，将系统过程噪声以及观测噪声建模为带有时变均值及协方差矩阵的相互独立的正态白噪声序列：$\left\{\begin{array}{c}\begin{array}{cc}E\[m\left(k\right)\]=q\left(k\right),& E\[\left(m\right(k)-q\left(k\right)){\left(m\right(j)-q(j\left)\right)}^T\]=Q\left(k\right){\mathrm{\δ}}_{kj}\end{array}\\ \begin{array}{cc}E\[n\left(k\right)\]=r\left(k\right),& E\[\left(n\right(k)-r\left(k\right)){\left(n\right(j)-r(j\left)\right)}^T\]=R\left(k\right){\mathrm{\δ}}_{kj}\end{array}\\ E\[\left(m\right(k)-q\left(k\right)){\left(n\right(j)-r(j\left)\right)}^T\]=0\end{array}\right.$EKF利用泰勒级数展开的思想通过求解非线性状态方程和观测方程的偏导数来将SLAM系统线性化；若k‑1时刻X(k‑1)的最优状态估计则X(k)的估计可按Kalman滤波基本方程求解；进而按照下述带时变噪声估值器的自适应EKF‑SLAM基本步骤流程执行：步骤A：执行航位推算的无人侦察机随着不断航行，利用所携带的环境感知传感器不断感知到环境中的特征，并提取所获取图像中的特征；步骤B：每次提取到特征需要判断当前特征是新特征或是无人侦察机已经观测过并融入地图中的旧特征，即完成数据关联过程；步骤C：如果数据关联结果表明所观测特征为地图中已存在特征，则根据对该特征的实际测量值与预测测量值之间的差异来实现无人侦察机状态及整个地图中所有特征位置的校正；具体如下：SLAM过程中包含了系统状态向量的前二阶矩：$E\[X\left(k\right)\]=\hat{X}\left(k\right)$$E\[\left(X\right(k)-\hat{X}(k\left)\right){\left(X\right(k)-\hat{X}(k\left)\right)}^T\]=P\left(k\right)$其中，表示无人侦察机及地图中特征状态的估计值，P(k)表示无人侦察机及特征状态估计误差的协方差矩阵；步骤C‑1：状态预测过程，利用状态方程对系统的状态估计及协方差矩阵进行预测：$\hat{X}\left(k\right|k-1)=F\left(k\right)X(k-1)+\hat{q}(k-1)---\left(9\right)$$P\left(k\right|k-1)=F\left(k\right)P(k-1)F{\left(k\right)}^T+W\left(k\right)\hat{Q}(k-1)W{\left(k\right)}^T---\left(10\right)$其中，$F\left(k\right)=\left[\begin{array}{cc}{F}_V\left(k\right)& 0_{8\×2n}\\ 0_{2n\×8}& I_{2n\×2n}\end{array}\right],W\left(k\right)=\left[\begin{array}{c}{W}_N\left(k\right)\\ 0_{2n\×4}\end{array}\right],$F_V(k)是非线性运动方程f对状态X_A在当前最优估计处的雅克比矩阵；W(k)是非线性状态方程f对噪声m(k)的雅克比矩阵；由时变噪声统计特性估值器递推获得：$\hat{q}\left(k\right)=\frac{1}{k}\underset{i=0}{\overset{k-1}{\Σ}}\[\hat{X}(i+1|i+1)-F\left(k\right)\hat{X}\left(i\right|i)\]---\left(11\right)$$\hat{Q}\left(k\right)=\frac{1}{k}\underset{i=0}{\overset{k-1}{\Σ}}\[K(i+1)\mathrm{\υ}(i+1)\mathrm{\υ}{(i+1)}^{T}K{(i+1)}^T+P(i+1|i+1)-F\left(i\right)P\left(i\right|i)F{\left(i\right)}^T\]---\left(12\right)$步骤C‑2：SLAM的状态校正通过速度、高度、艏向及环境感知传感器的测量，来实现无人侦查机状态及地图中特征状态的更新，测量更新方程为：$\hat{X}\left(k\right)=\hat{X}\left(k\right|k-1)+K\mathrm{\υ}\left(k\right)---\left(13\right)$P(k)＝[I‑K(k)H(k)]P(k|k‑1)   (14)K(k)＝P(k|k‑1)H(k)^T(S(k))^‑1S(k)   (15)$\mathrm{\υ}\left(k\right)=Z\left(k\right)-H\left(k\right)\hat{X}\left(k\right|k-1)-\hat{r}\left(k\right)---\left(16\right)$$S\left(k\right)=H\left(k\right)P\left(k\right|k-1)H{\left(k\right)}^T+\hat{R}(k-1)---\left(17\right)$其中，H是非线性传感器观测方程h对X的偏导数的雅克比矩阵，H根据测量传感器的不同而具有相应的不同形式；和是由时变噪声统计估值器递推获得：$\hat{r}\left(k\right)=\frac{1}{k}\underset{i=0}{\overset{k-1}{\Σ}}\[Z(i+1)-H(i+1)\hat{X}(i+1|i)\]---\left(18\right)$$\hat{R}\left(k\right)=\frac{1}{k}\underset{i=0}{\overset{k-1}{\Σ}}\[\mathrm{\υ}(i+1)\mathrm{\υ}{(i+1)}^T-H(i+1)P(i+1|i)H{(i+1)}^T\]---\left(19\right)$将式(9)～(19)迭代计算即可实现噪声统计特性的递推估计，进而实现带时变噪声统计特性估值器的自适应EKF‑SLAM过程。