基于PSTCSDREF算法的无人机INSBDS组合导航方法与流程

本发明涉及无人机导航技术领域，具体涉及一种基于pstcsdref算法的无人机insbds组合导航方法。

背景技术：

无人机主要是通过旋翼的高速旋转得到升力并实现操控，可以在高低起伏的复杂地形下作业，在智能化发展的当代得到了广泛的应用。导航系统是无人机能够实现自主飞行、完成机动任务的必要条件，采用多种传感器对同一信息源进行测量，通过不同传感器量测特性的互补，获得更高精度的导航信息，以此来控制飞行器的飞行姿态。其中，ins/bds组合导航系统就是一种优势互补的组合导航系统。该组合导航系统以具有较高自主性的惯性导航系统(inertialnavigationsystem,缩写为ins)为主，并用通过bds(beidousystem，缩写为bds)得到的定位数据进行修正，来保证ins/bds组合导航系统具有较高的测量精度。由于无人机体积、载重、成本有限，无人机上通常都使用低成本小型化的低精度机载导航传感器来提供导航信息，为了使用上述低精度的传感器数据得到高精度的导航信息，就需要在对传感器获取的信息进行融合时作滤波处理。

对于ins/bds组合导航系统，通常采用扩展卡尔曼滤波(extendedkalmanfilter,缩写为ekf)或无迹卡尔曼滤波(unscentedkalmanfilter,缩写为ukf)来进行数据信息融合。由于ekf是利用雅克比矩阵将非线性函数展开成taylor级数并略去二阶及以上项，线性化所产生的截断误差会降低滤波精度和稳定性，因此在实际工程中，ekf处理弱非线性系统时效果比较好。而ukf的估计精度相对于ekf较高，但ukf的计算量比较大，不利于实时滤波导航。状态相关riccati方程滤波器(sate-dependentriccatiequationfilter,缩写为sdref)通过对非线性状态方程进行分解，得到每一步的riccati方程和状态估计。sdref保留了系统的非线性，具有更高的精度且计算量也较小。上述滤波方法的一个前提是该非线性系统是能精确建模的，但是实际应用中，系统模型会受到干扰，具有不确定性(比如ins的漂移、传感器的测量偏差、系统的未知扰动等)。这些不确定性会严重影响组合导航系统的估计精度，甚至会导致组合导航系统的精度发散。

技术实现要素：

本发明提供一种基于pstcsdref(partlystrongtrackingconsiderstate-dependentriccatiequationfilter，缩写为pstcsdref)算法的无人机insbds组合导航方法以解决现有的导航系统滤波精度低，滤波稳定性差的技术问题。

为解决上述技术问题，本发明采用如下技术方案：

设计一种基于pstcsdref算法的无人机insbds组合导航方法，包括：

步骤1：测量无人机组合导航系统的初始参数；

步骤2：将初始参数带入无人机的状态方程和量测方程，量测方程包括量测偏差；

步骤3：获取无人机状态的一步预测、一步预测的协方差、滤波增益矩阵；

步骤4：利用一步预测、协方差和滤波增益矩阵，根据卡尔曼滤波算法，获得下一时刻的无人机状态更新；

步骤5：结合无人机的当前时刻状态与下一时刻的预测状态，获取协方差更新；

步骤6：将下一时刻的预测状态作为基础带入步骤3中，依次执行步骤3-步骤5，得出更下一时刻的无人机预测状态，如此循环迭代，得出无人机的运行状态。

进一步的，上述初始参数包括：无人机的东方向误差角、北方向误差角、天方向误差角；载体的东方向的速度、北方向的速度、天方向的速度；无人机所在的纬度、经度、高度；陀螺仪的东方向常值漂移、北方向常值漂移、天方向常值漂移；加速度计的东方向常值漂移、北方向常值漂移、天方向常值漂移；初始状态估计误差方差矩阵、初始误差角方差矩阵、初始速度方差矩阵、初始位置方差矩阵；陀螺仪的常值漂移方差矩阵、加速度计的常值漂移方差矩阵。

进一步的，无人机的状态方程表达式为：

式中，φe、φn和φu分别为东方向误差角、北方向误差角和天方向误差角，ve、vn和vu是无人机在东、北和天方向的速度分量，λ和h是无人机在纬度、经度和高度的位置分量，陀螺仪分别有东方向常值漂移εe、北方向常值漂移εn、天方向常值漂移εu，加速度计分别有东方向常值漂移δe、北方向常值漂移δn、天方向常值漂移是陀螺仪和加速度计随机误差，δve、δvn、和δh是ins和bds在东方向的速度差、北方向的速度差、纬度差和高度差，fe、fn和fu是加速度计在东方向、北方向和天方向测量值的分量。

进一步的，导航系统中bds的量测信息为：

z＝[vge,vgn,vgu,pge,pgn,pgu]^t(2)

ins/bds组合导航系统的量测方程为：

z＝h(x)+θ+v(3)

式中，θ＝[bvge,bvgn,bvgu,bpge,bpgn,bpgu]^t是量测偏差，bvge、bvgn和bvgu分别为在导航坐标系东方向、北方向、天方向的速度量测偏差，bpge、bpgn和bpgu分别为在导航坐标系东方向、北方向、天方向的位置量测偏差，量测噪声为v＝[vvge,vvgn,vvgu,vpge,vpgn,vpgu]^t，vvge、vvgn和vvgu分别为在导航坐标系东方向、北方向、天方向的bds速度量测值的随机误差，vpge、vpgn和vpgu分别为在导航坐标系东方向、北方向、天方向bds位置量测值的随机误差。

进一步的，对状态方程进行离散化处理，方便计算机运行，离散化后的状态方程为：

式中，dt为采样时间间隔，xk-1是tk-1时刻的状态向量，xk是tk时刻的状态向量。

进一步的，步骤2中无人机的初始状态表示为：

式中，状态估计误差与偏差的互协方差其初始值为c0＝0，状态估计误差满足等式

进一步的，步骤3中无人机状态的一步预测为：

一步预测的协方差为：

式中，λk为自适应渐消因子，自适应渐消因子的计算方法为：

式中，ok和uk的表达式为：

滤波增益矩阵为：

进一步的，步骤4中无人机的状态更新为：

进一步的，在步骤5中所述协方差更新为

与现有技术相比，本发明的有益技术效果在于：

1、本发明采用pstcsdref通过对非线性状态方程进行分解，转换为sdc形式，得到线性化的状态方程，避免了雅可比矩阵的计算，得到相应的riccati方程，以此来得到状态估计。

2、本发明使用“consider”方法将偏差的统计信息合并到状态估计方程中，而不直接估计它们，以此来减少偏差的负面影响并获得较好滤波效果；如果参数数量很大，该方法可以节省计算和处理成本。

3、本发明将自适应渐消因子引入到pstcsdref中，能够降低模型不确定性的负面影响，提高了本发明的滤波精度。

4、本发明使用pstcsdref算法鲁棒性强，信息融合结果更接近于真实状态，提高集成导航系统的准确性。

附图说明

图1为本发明一种实施例的方法流程示意图；

图2为本发明一种实施例的无人机东向位置误差仿真图；

图3为本发明一种实施例的无人机北向位置误差仿真图；

图4为本发明一种实施例的无人机天向位置误差仿真图；

图5为本发明一种实施例的无人机东向速度误差仿真图；

图6为本发明一种实施例的无人机北向速度误差仿真图；

图7为本发明一种实施例的无人机天向速度误差仿真图。

具体实施方式

下面结合附图和实施例来说明本发明的具体实施方式，但以下实施例只是用来详细说明本发明，并不以任何方式限制本发明的范围。

以下实施例中所涉及或依赖的程序均为本技术领域的常规程序或简单程序，本领域技术人员均能根据具体应用场景做出常规选择或者适应性调整。

实施例1：一种基于pstcsdref算法的无人机insbds组合导航方法，参见图1，包括以下三个步骤：

步骤一、建立无人机ins/bds组合导航系统的直接法状态方程和量测方程

组合导航系统的直接法即为直接以导航系统输出的导航参数作为状态值，并使用卡尔曼滤波进行估计。采用直接法对导航系统参数进行估计，选取东-北-天坐标系作为导航坐标系，将加速度计和陀螺仪产生的漂移扩维进系统的状态，定义系统状态向量为组合导航系统的状态方程如下：

式中，φe、φn和φu分别为东方向误差角、北方向误差角和天方向误差角，ve、vn和vu是载体在东、北和天方向的速度分量，λ和h是载体在纬度、经度和高度的位置分量，陀螺仪分别有东方向常值漂移εe、北方向常值漂移εn、天方向常值漂移εu，加速度计分别有东方向常值漂移δe、北方向常值漂移δn、天方向常值漂移δu，是组合导航系统中的陀螺仪和加速度计随机误差，δve、δvn、和δh是ins和bds在东方向的速度差、北方向的速度差、纬度差和高度差，fe、fn和fu是加速度计在东方向、北方向和天方向测量值的分量。

bds测量无人机的速度和位置，其量测信息为z＝[vge,vgn,vgu,pge,pgn,pgu]^t，组合导航系统的量测方程如下所示：

z＝h(x)+θ+v(2)

式中，θ＝[bvge,bvgn,bvgu,bpge,bpgn,bpgu]^t是量测偏差，bvge、bvgn和bvgu分别为在导航坐标系东方向、北方向、天方向的速度量测偏差，bpge、bpgn和bpgu分别为在导航坐标系东方向、北方向、天方向的位置量测偏差，量测噪声为v＝[vvge,vvgn,vvgu,vpge,vpgn,vpgu]^t，其中，vvge、vvgn和vvgu分别为在导航坐标系东方向、北方向、天方向bds速度量测值的随机误差，vpge、vpgn和vpgu分别为在导航坐标系东方向、北方向、天方向bds位置量测值的随机误差。

随后将非线性系统的方程等式(1)和等式(2)转换为状态相关系数(statedependentcoefficient，sdc)的形式：

其中，f(x)＝f(x)x，h(x)＝h(x)x。

步骤二、离散化组合导航系统方程

对ins/bds组合导航系统的状态方程转换为时间离散的形式，以便于在计算机上进行计算，离散状态方程：

式中，dt为采样时间间隔，xk-1是tk-1时刻的状态向量，xk是tk时刻的状态向量。

由于量测方程本身是时间离散的，故无需再离散化处理。

将离散化后的状态方程和测量方程抽象化得到带有偏差的非线性离散状态方程和量测方程：

式中，偏差θk满足zk为tk时刻的量测值，f和h分别为非线性状态方程和量测方程，wk为方差为qk的过程噪声，vk为方差为rk的量测噪声。wk和vk满足条件：

式中，δkj为kroneckerδ函数，当k＝j时，δkj＝1，当k≠j时，δkj＝0，为wk的转置矩阵。

将由(5)表示的非线性系统转换为sdc(state-dependentcoefficients,sdc)的形式，得到的方程：

式中，f(xk-1,θk-1)和ν(xk,θk)为状态转移矩阵，g(xk-1,θk-1)和m(xk,θk)为偏差系数矩阵。

由于不确定参数无法在实际测量得到，因此根据经验得到不确定性参数θ在滤波过程中仅知道其参考值和协方差步骤三、pstcsdref算法及ins/bds组合导航输出

本发明中pstcsdref算法实现步骤如下：

1、对ins/bds组合导航系统模型初始化

即将根据经验估计的初始参数代入系统模型，设置初始状态和协方差，以便于进行一步预测，组合导航系统的初始参数为

式中，状态估计误差与偏差的互协方差其初始值为c0＝0，状态估计误差满足等式

初始状态初始数学平台在东方向误差角φe0＝-0.000146°、北方向误差角φn0＝0.000145°、天方向误差角φu0＝-0.005818°；载体的东方向的速度ve0＝0.1m/s、北方向的速度vn0＝0.1m/s、天方向的速度vu0＝0.1m/s；载体所在的纬度经度λ0＝2.029978°、高度h0＝3m；陀螺仪东方向常值漂移εe0＝1.45×10^-9(°)/h、北方向常值漂移εn0＝1.45×10^-9(°)/h、天方向常值漂移εu0＝1.45×10^-9(°)/h；加速度计东方向常值漂移δe0＝9.78×10^-6g、北方向常值漂移δn0＝9.78×10^-6g、天方向常值漂移δu0＝9.78×10^-6g。

初始状态估计误差方差矩阵为初始数学平台误差角方差阵，为初始速度方差阵，为初始位置方差阵，为初始陀螺仪常值漂移方差阵，为初始加速度计常值漂移方差阵。

2、对状态一步预测进行计算

由第k-1步的状态估计值来计算第k步的一步预测状态估计

由第k-1步的互协方差ck-1来计算第k步的一步预测互协方差ck/k-1

式中，ck-1是在时间为k-1时状态估计误差矩阵与偏差之间的互协方差矩阵。

3.对一步预测协方差进行计算

由第k-1步的状态估计误差方差pk-1来计算第k步的一步预测方差pk/k-1

式中，λk为自适应渐消因子，自适应渐消因子的计算方法为：

式中，ok和uk的计算方法如下所示：

4.计算滤波增益矩阵

通过一步预测协方差和测量噪声方差矩阵获取kalman滤波器的增益矩阵

式中，ωk的计算方法为：

5.计算状态更新

6.计算协方差更新

状态估计误差方差为：

状态估计误差与偏差的互协方差为：

通过以上6步循环迭代，可以得到ins/bds组合导航系统的姿态误差、速度和位置，其中无人机姿态信息为载体在东方向、北方向和天方向的状态量误差角、和，载体速度信息包括载体的东方向的速度、北方向的速度和天方向的速度，载体位置信息包括纬度、经度、高度。

实验例：参见图2至图7，表示本申请pstcsdref算法和传统的sdref算法得出的无人机组合导航系统在动、北、天方向上的位置及速度误差。可以看出pstcsdref算法得出的误差值都在o附近，而sdref算法偏差较大，特别是在400s和1400s，更是出现了巨大的毛刺。该实验例中采样时间为h＝1[s]，当k＝1时，对应的时间为t＝0[s]；当k＝2时，对应的时间为t＝1[s]，每步的对应时间以此类推。在观测次数为n＝50、总的采样时间为统计获得

上面结合附图和实施例对本发明作了详细的说明，但是，所属技术领域的技术人员能够理解，在不脱离本发明宗旨的前提下，还可以对上述实施例中的各个具体参数进行变更，形成多个具体的实施例，均为本发明的常见变化范围，在此不再一一详述。