基于状态变换卡尔曼滤波的DVL_SINS组合导航算法.pdf

第 卷第 期 年 月系统工程与电子技术 文章编号：（）网址：收稿日期：；修回日期：；网络优先出版日期：。网络优先出版地址：基金项目：国家自然科学基金（）资助课题通讯作者引用格式：靳凯迪，柴洪洲，宿楚涵，等基于状态变换卡尔曼滤波的 组合导航算法系统工程与电子技术，（）：犚犲 犳 犲 狉 犲 狀 犮 犲犳 狅 狉犿犪 狋：，（）：基于状态变换卡尔曼滤波的犇犞犔犛 犐犖犛组合导航算法靳凯迪，柴洪洲，宿楚涵，向民志，李明（战略支援部队信息工程大学地理空间信息学院，河南 郑州 ；郑州航天宏图北斗应用技术研究院有限公司，河南 郑州 ）摘要：针对捷联惯性导航系统（，）误差定义存在的坐标系不一致的问题，将速度误差统一投影在真实导航坐标系中，基于状态变换 滤波推导了更严密的 姿态、速度和位置误差方程。进一步地，在新定义的速度误差和 误差方程的基础上，构建了改进的多普勒计程仪（，）组合导航卡尔曼滤波状态模型和量测模型。仿真和船载实验表明，与常规 卡尔曼滤波模型相比，所提算法可以处理传统 误差模型中坐标系不一致问题的影响，可在一定程度上提高组合导航系统的性能。关键词：捷联惯性导航系统；多普勒计程仪；组合导航；误差方程；卡尔曼滤波中图分类号：文献标志码：犇犗犐：犛 狋 犪 狋 犲狋 狉 犪 狀 狊 犳 狅 狉犿犪 狋 犻 狅 狀犓犪 犾 犿犪 狀犳 犻 犾 狋 犲 狉犳 狅 狉犇犞犔犛 犐犖犛犻 狀 狋 犲 犵 狉 犪 犾狀 犪 狏 犻 犵 犪 狋 犻 狅 狀狊 狔 狊 狋 犲犿 ，（犐 狀 狊 狋 犻 狋 狌 狋 犲狅 犳犌犲 狅 狊 狆犪 狋 犻 犪 犾犐 狀犳 狅 狉犿犪 狋 犻 狅 狀，犛 狋 狉 犪 狋 犲 犵 犻 犮犛狌狆狆 狅 狉 狋犉狅 狉 犮 犲犐 狀犳 狅 狉犿犪 狋 犻 狅 狀犈狀犵 犻 狀 犲 犲 狉 犻 狀犵犝狀 犻 狏 犲 狉 狊 犻 狋 狔，犣犺 犲 狀犵 狕 犺 狅 狌 ，犆犺 犻 狀 犪；犘犐犈犛犃犜犐 狀 狊 狋 犻 狋 狌 狋 犲狅 犳犃狆狆 犾 犻 犲 犱犅犲 犻 犱 狅 狌犖犪 狏 犻 犵犪 狋 犻 狅 狀犜犲 犮 犺 狀 狅 犾 狅 犵 犻 犲 狊犪 狋犣犺 犲 狀犵 狕 犺 狅 狌，犣犺 犲 狀犵 狕 犺 狅 狌 ，犆犺 犻 狀 犪）犃犫 狊 狋 狉 犪 犮 狋：（），（），犓犲 狔狑狅 狉 犱 狊：（）；（）；引言水下无人自主航行器（，）在海洋测绘、水下资源探索和水下目标探测等任务中发挥着越来越重要的作用。获取高精度的姿态、速度和位置等导航参数是高效和安全完成任务的重要前提。捷联惯性导航系统（，）具有自主性、隐蔽性和主动性等优势，可为提供丰富的导航参数。然而，受惯性测量元件（，）误差的影响，定位误差会随时间迅速第 期靳凯迪等：基于状态变换卡尔曼滤波的 组合导航算法 累积，因此必须使用辅助传感器修正 累积误差。多普勒计程仪（，）组合导航系统是应用最为广泛的水下定位系统。根据传感器耦合方式的不同，组合导航可分为松组合和紧组合，二者分别使用三维速度和波束频移作为 滤波的量测信息。然而，由于当前大部分仅能提供三维速度，在实际情况中应用最为广泛的仍为松组合方式。近年来，许多学者针对 组合导航 滤波中的误差模型、非完整性约束和自适应滤波等问题展开了广泛研究，有效提升了 组合导航系统的定位性能 。组合导航卡尔曼滤波中的状态方程由 误差方程构成。根据姿态误差表达方式的不同，误差方程可分为欧拉角误差模型和四元数误差模型 。针对传统 误差模型不严密的问题，文献 提出了一种基于矢量运算坐标系一致的误差构建思想。在其基础上，文献 指出，传统 速度误差的定义只考虑了实际速度与理论速度大小的差异，而忽略了矢量坐标系不一致的影响。进一步，文献 通过定义新的速度误差提出了状态变换卡尔曼滤波，并将其成功应用于全球卫星导航（，）系统 组合导航、视觉 组合导航和 初始对准等陆地场景 。文献 和文献 将速度误差定义在计算导航系中，分别推导了 阻尼 非线性误差模型的欧拉角形式和四元数形式，并将其应用于 大失准角对准中。本文延续统一矢量运算中坐标系的思想，将状态变换卡尔曼滤波引入 组合导航。首先，指出传统 误差方程中姿态误差方程存在的坐标系不一致问题，进而构建了更为严密的 误差方程；进一步，基于新定义的速度误差推导了改进的 组合导航卡尔曼滤波状态方程和量测方程；最后，利用仿真数据和船载实测数据验证了本文所提算法的有效性。坐标系定义本文坐标系定义如下：（）导航坐标系（狀系）：定义“东 北 天（）”当地水平坐标系为导航坐标系；（）计算导航坐标系（狀 系）：实际计算时使用的导航坐标系，狀 系与狀系间的欧拉角称为失准角；（）载体坐标系（犫系）：定义“右 前 上（）”载体坐标系与 坐标系重合，犫系与狀系间的欧拉角称为 姿态角，包括俯仰角、横滚角和偏航角；（）坐标系（犱系）：原点为的几何中心，狓 狔 狕轴分别指向框架的“右 前 上”方向，犱系与犫系间的欧拉角称为的安装偏差角，犫系原点至犱系原点的位移称为的杆臂误差 犾；（）地球坐标系（犲系）：原点位于地球质心，狓轴指向赤道与本初子午线的交点，狕轴指向北极点，狔轴与狓轴和狕轴构成右手坐标系；（）地心惯性坐标系（犻系）：原点位于地球质心，狓轴指向春分点，狕轴指向北极点，狔轴与狓轴和狕轴构成右手坐标系，犻系不随地球自转而转动。状态变换犛 犐 犖犛误差模型 传统犛 犐 犖犛误差模型 姿态可表示为欧拉角或四元数。在小失准角假设下，的欧拉角误差模型和四元数误差模型均为线性，但四元数各分量的物理意义不明确，而且增加了误差模型的维度，因此本文选择欧拉角（失准角）描述 的姿态误差。传统 姿态误差模型为珟狀犻 狀狀犻 狀犆狀 犫犫犻 犫（）式中：上标表示微分；上标表示含有误差的量；狀犻 狀狀犻 犲狀犲 狀为狀系相对于惯性系的角速度在狀系上的投影；狀犻 犲为地球自转角速度在狀系上的投影；狀犲 狀表示由载体速度引起的角速度在狀系上的投影；犆狀 犫犆狀 狀犆狀犫为 获得的姿态矩阵；狀犻 狀珟狀犻 狀狀犻 狀为珟狀犻 狀的误差；犫犻 犫和犫犻 犫分别为的陀螺量测值及其误差。速度误差模型为 狏狀（犆狀 犫犳犫）（珟狀犻 犲珟狀犲 狀）狏狀（狀犻 犲狀犲 狀）狏狀犆狀 犫 犳犫 犵狀（）式中：狏狀狏狀狏狀表示 的速度误差；犳犫为中加速度计的比力量测值；犳犫犳犫犳犫为加速度计测量误差；犵狀为由 位置误差引起的重力误差，一般可忽略不计。位置误差模型为狆犕狆 狏狏狀犕狆 狆 狆（）式中：狆犔，犺为 的位置误差；犔、和犺分别为 的纬度、经度和高程；犕狆 狏和犕狆 狆的具体形式为犕狆 狏珟犚犕犺 珟犔珟犚犕犺熿燀燄燅（）犕狆 狆狏狀犖珟犚犕犺狏狀犈 珟犔 珟犔珟犚犕犺 狏狀犈 珟犔珟犚犕犺熿燀燄燅（）式中：犚犕犺犚犕犺；犚犕犺犚犖犺；犚犕和犚犖分别为 所在位置的子午圈曲率半径与卯酉圈曲率半径。改进的犛 犐 犖犛误差模型文献 指出，实际计算的速度狏狀在计算导航坐标系中，而理论速度狏狀在真实导航系中。由于狏狀和狏狀不在同一坐标系，将 狏狀定义为狏狀狏狀狏狀只考虑了向量大小的不一致，而忽略了向量坐标系的不一致。因此，定义新的速度误差 为 狏狀犆狀狀 狏狀狏狀 狏狀狏狀（）由珟狀犲 狀的定义可知珟狀犲 狀与狏狀是相关的。传统上，将珟狀犲 狀的误差定义为加性误差：狀犲 狀珟狀犲 狀狀犲 狀犕犪 狏狏狀犕狆（）其中，犕犪 狏珟犚犕犺珟犚犕犺 珟犔珟犚犕犺熿燀燄燅（）系统工程与电子技术第 卷犕狏狀犖珟犚犕犺狏狀犈珟犚犕犺狏狀犈 珟犔珟犚犕犺 狏狀犈 珟犔珟犚熿燀燄燅犕犺（）根据式（）定义的速度误差，可将传统速度误差表示为 狏狀 狏狀狏狀（）将式（）代入式（）可得狀犲 狀犕犪 狏狏狀犕 狆犕犪 狏（狏狀）狀犲 狀，犕犪 狏（狏狀）（）结合式（）和式（），得到 狏狀表示的珟狀犲 狀为珟狀犲 狀狀犲 狀狀犲 狀，犕犪 狏（狏狀）（）将式（）代入式（），可得犕犪 狏（狏狀）珟狀犻 狀犕犪 狏（狏狀）狀犻 犲犕犪 狏狏狀犕 狆犆狀 犫犫犻 犫（）式中：狀犻 犲 犻 犲 珟犔 犻 犲 珟犔熿燀燄燅 狆犕 狆（）将式（）代入式（），得到新的姿态误差方程：犕犪 狏（狏狀）珟狀犻 狀犕犪 狏（狏狀）犕犪 狏狏狀犕犪 狆 狆犆狀 犫犫犻 犫（）式中：犕犪 狆犕犕。对式（）两侧微分，可得 狏狀 狏狀狏狀狏狀（）结合式（）、式（）和速度微分方程，式（）可改写为 狏狀（犆狀 犫犳犫）（珟狀犻 犲珟狀犲 狀）狏狀（狀犻 犲狀犲 狀）狏狀犆狀 犫 犳犫 犵狀犆狀 犫犳犫（珟狀犻 犲珟狀犲 狀）狏狀珘犵狀狏狀 犕犪 狏（狏狀）珟狀犻 狀狀犻 犲狀犲 狀，犆狀 犫犫犻 犫（珘犵狀）（珟狀犻 犲珟狀犲 狀）狏狀（狏狀）犕犪 狏（狏狀）（狏狀）（珟狀犻 狀）（珟狀犻 犲珟狀犲 狀）狏狀（狀犻 犲狀犲 狀）狏狀犆狀 犫 犳犫 犵狀狀犲 狀，狏狀狏狀犆狀 犫犫犻 犫（）将式（）和式（）代入式（），有 狏狀狀犻 犲狀犲 狀，犕犪 狏（狏狀）狏狀犆狀 犫 犳犫 犵狀狀犲 狀，狏狀狏狀犆狀 犫犫犻 犫（珘犵狀）（珟狀犻 犲珟狀犲 狀）狏狀（狏狀）犕犪 狏（狏狀）（狏狀）（珟狀犻 狀）（珟狀犻 犲珟狀犲 狀）（狏狀狏狀）（珟狀犻 犲珟狀犲 狀）狏狀狀犻 犲狏狀犆狀 犫 犳犫 犵狀狏狀犆狀 犫犫犻 犫（珘犵狀）（珟狀犻 犲珟狀犲 狀）狏狀（狏狀）（珟狀犻 狀）（珟狀犻 犲珟狀犲 狀）（狏狀）（）由于（珟狀犻 犲珟狀犲 狀）狏狀（珟狀犻 犲珟狀犲 狀）（狏狀）（狏狀）（珟狀犻 犲珟狀犲 狀）（）将式（）和式（）代入式（）中并化简，得到新的速度误差方程：狏狀（珘犵狀）（狏狀）（珟狀犻 犲）狏狀狀犻 犲（珟狀犻 犲珟狀犲 狀）狏狀狏狀犆狀 犫犫犻 犫犆狀 犫 犳犫 犵狀（）将式（）代入式（），得到新的位置误差方程：狆犕狆 狏（狏狀）犕狆 狏狏狀犕狆 狆 狆（）式（）、式（）和式（）构成了新的状态变换 误差方程。与文献 提出的状态变换误差模型相比，本文指出姿态误差方程中的珟狀犲 狀也存在坐标系不一致的问题，因此误差模型更为严密。犇犞犔犛 犐 犖犛组合导航卡尔曼滤波 卡尔曼滤波状态方程测速精度受水体温度、盐度和波束宽度等因素影响，在实际应用中一般将观测值建模为狏犱（犽）狏犱狑犱（）式中：犽是建模为常值的刻度因子；狑犱为高斯白噪声。考虑和 的误差参数以及犱系与犫系间的安装偏差角和杆臂误差，在 组合导航卡尔曼滤波中选择 维状态向量：犡，（狏狀），狆，（犫），（犫），犾，犽（）式中：犫和犫分别为可视为随机常值的陀螺零偏和加速度计零偏。由于狔不可观测，在实际应用中可将其从式（）中排除。同时，若事先准确标定了杆臂误差或安装偏差角，也可不将其列为状态参数 。因此，滤波状态方程为犡犉狋熿燀燄燅 犡犌狋犠犫（）式中：犉狋是由 误差方程构成的状态转移矩阵；犌狋为系统噪声驱动矩阵；犠犫为由陀螺和加速度计的白噪声构成的状态噪声。根据式（）、式（）和式（），状态转移矩阵和系统噪声驱动矩阵的具体形式为犉狋犕犪 狏（狏狀）珟狀犻 狀犕犪 狏犕犕犆狀 犫（珘犵狀）（狏狀）（珟狀犻 犲）（珟狀犻 犲珟狀犲 狀）（狏狀）犕（狏狀）犆狀 犫犆狀 犫犕狆 狏（狏狀）犕狆 狏犕狆 狆熿燀燄燅，犌狋犆狀 犫（狏狀）犆狀 犫犆狀 犫熿燀燄燅（）卡尔曼滤波量测方程 松组合中以三维速度作为卡尔曼滤波的量测信息。考虑的安装偏差角与杆臂误差，由式（）可得狏犱（犽）犆犱犫（犆犫狀狏狀犫犲 犫 犾）狑犱（犽）（犐）犆犫狀（犆狀狀 狏狀 狏狀）犫犲 犫 犾狑犱犆犫狀 狏狀犆犫狀 狏狀（犆犫狀 狏狀）犫犲 犫 犾犆犫狀 狏狀 犽狑犱（）由式（），得到 组合导航量测方程为犣犆犫狀 狏狀狏犱，犆犫狀，（犆犫狀 狏狀），（犫犲 犫），犆犫狀 狏狀犡狑犱（）第 期靳凯迪等：基于状态变换卡尔曼滤波的 组合导航算法 综上，组合导航卡尔曼滤波数据融合过程如图所示。图 组合导航数据融合过程 实验结果与分析 仿真实验为验证 状态变换 滤波算法的有效性，首先设计如图所示的仿真轨迹进行实验验证。仿真轨迹总时长为，包含如表所示的加速、减速、匀速、横滚和转向等典型运动状态。轨迹初始位置为北纬、东经 、高程，初始速度为，初始姿态角为，。轨迹的初始水平失准角设为，初始天向失准角为 ，个方向的初始速度误差均为。图仿真轨迹 表仿真轨迹运动状态犜 犪 犫 犾 犲犕狅 狋 犻 狅 狀狊 狋 犪 狋 狌 狊狅 犳狊 犻 犿狌 犾 犪 狋 犻 狅 狀狋 狉 犪 犼 犲 犮 狋 狅 狉 狔运动状态数值数据区间加速、匀速 、横滚 、转向 、实验中 和的具体参数设置为：陀螺常值零偏 ，角度随机游走 槡，加速度计常值零偏 犵，速度随机游走 犵槡，采样频率 ；测速误差，安装偏差角 ，杆臂误差，刻度系数，采样频率。为说明本文算法的有效性，使用常规 组合导航 滤波（以“常规算法”表示）作为对比 。在相同条件下执行两种算法，分别进行 次蒙特卡罗仿真实验。在姿态误差上，由于两种算法的水平姿态角估计精度均较高，在此仅对比天向失准角的估计效果。图给出了 次蒙特卡罗实验中两种算法的天向失准角平均值曲线。从图可见，本文算法的收敛速度更快。在 初始天向失准角下，本文算法的平均天向失准角在 左右收敛到 以下，而常规方式在 左右收敛到 以下。导航结束时，常规算法的天向失准角为 ，本文算法的天向失准角为 。因此，本文算法在一定程度上提高了 组合导航的姿态精度。图天向失准角平均值 松组合直接以三维速度作为量测值，组合导航系统中速度误差具有较强的可观测性。图给出了两种方案的 个方向速度误差的平均误差曲线。从图可见，两种算法的速度误差在的约束下迅速收敛，且本文算法的水平速度估计效果优于常规算法，在测速精度和稳定性上均有一定程度的提升。经过初始收敛阶段，在 方向上，常规算法的最大速度误差分别为 和 ，而本文算法分别为 和 ，本文算法在导航前期有较高的速度精度。随着导航进行，二者差异逐渐减小，在对准结束时，两种算法在 方向上的速度误差均为 和 。对比式（）和式（）可见，由于在新的速度误差方程中由更稳定的重力矢量代替了式（）中的比力信息，因此 滤波状态转移矩阵更加稳定。系统工程与电子技术第 卷图速度平均误差 图给出了两种方案 个方向的平均位置误差曲线。由于 系统中位置误差不可观，因此解算的位置误差是速度误差累积的结果。从图可见，常规算法的 个方向的最大位置误差分别为 、和，本文算法个方向的最大位置误差分别为，和。在导航初始阶段，本文算法的位置误差发散得到了一定程度的改善。在导航结束时，两种算法的定位误差的差异在毫米级。图位置平均误差 图图给出了两种方案的的刻度系数、安装偏差角和杆臂误差的 次实验平均值曲线。从图中可见，对于可观性较强的状态参数，如狓轴狕轴安装偏差角、刻度系数和杆臂误差，两种算法的估计效果较为接近，均可估计出添加的误差参数。对于可观性较差的状态参数，两种算法有一定差异，且均无法准确估计。图刻度系数平均值 图安装偏差角平均值 图杆臂误差平均值 第 期靳凯迪等：基于状态变换卡尔曼滤波的 组合导航算法 船载实验为进一步分析本文算法的性能，使用武汉严西湖的船载实测数据进行实验验证。船载实验平台如图所示，实验设备包括流动站及基准站、光纤和 。与的标称参数为：陀螺零偏稳定性 ，角度随机游走 槡，加速度计零偏稳定性 犵，速度随机游走 犵槡，采样频率 ；测速误差，分辨率，采样频率。图船载平台及设备 实验使用高精度后处理软件 （）的 前后向平滑紧组合的输出作为参考基准。由 结果获得轨迹的初始位置为北纬 ，东经 ，高程。船载轨迹如图 所示。图 武汉湖试轨迹 为了避免杆臂误差、安装误差角和刻度系数对导航参数的影响，使用文献 算法对实测数据中的误差参数进行了预标定。实验中，在 参考值的基础上，设置轨迹的初始水平失准角为 ，初始天向失准角为，个方向速度误差均为。使用常规算法与本文算法分别对该轨迹进行处理。图 给出了两种算法估计的天向失准角对比。从图 可见，在导航前期二者的差异较为显著，本文算法的最大天向失准角误差为 ，常规算法为 。随着导航进行，二者差异逐渐减小，在导航结束时常规算法和本文算法的天向失准角分别为 和 。因此，在补偿了坐标系不一致误差后，组合导航系统估计的姿态精度更高。图 湖试中的天向失准角 图 给出了两种算法的速度误差估计曲线。从图 可见，两种算法的天向速度有基本一致的估计精度。在 方向上，常规算法的最大速度误差分别为 和 ，而本文算法的最大速度误差分别为 和 。因此，本文算法在导航前期的测速精度较高，随着导航进行，两种算法速度误差的差异逐渐减小。图 湖试中的速度误差 系统工程与电子技术第 卷图 给出了两种算法的位置误差估计曲线。从图 可见，常规算法和本文算法的水平最大位置误差分别为 和 。当导航结束时，常规算法和本文算法的水平位置误差分别为 和 。这是由于本文算法的速度误差较小，而位置误差受速度误差累积的影响。统计两种算法估计的姿态、速度和位置均方根差（，）如表所示。从表可见，本文算法的姿态、速度和位置的精度较常规算法均有一定程度的提升。其中，天向失准角降低了，东向速度降低了，北向速度降低了。本文算法的东向位置误差和北向位置误差的分别降低了 和。图 湖试中的位置误差 表湖试中导航参数犚犕犛犈统计结果犜 犪 犫 犾 犲犚犕犛犈狅 犳狀 犪 狏 犻 犵 犪 狋 犻 狅 狀狆 犪 狉 犪犿犲 狋 犲 狉 狊犻 狀犾 犪 犽 犲狋 犲 狊 狋方案失准角（）速度误差（）位置误差常规算法 本文算法 结论为了提高 组合导航系统的定位精度，考虑 速度误差中坐标系不一致的问题，提出了一种基于状态变换 滤波的 组合导航算法。在分析传统 误差方程中存在坐标系不一致误差的基础上，详细推导了改进的状态变换 误差方程；进一步地，构建了 组合导航 滤波方程。实验结果表明，该算法可以有效处理传统速度误差中坐标系不一致的影响，在一定程度上提高了 组合导航系统的精度。参考文献，（）：，（）：，（）：，：，（）：靳凯迪，柴洪洲，宿楚涵，等 组合导航技术发展现状及趋势导航定位学报，（）：，（）：，（）：，（）：，：，（）：，：，（）：，（）：，第 期靳凯迪等：基于状态变换卡尔曼滤波的 组合导航算法 ，徐晓苏，潘永飞，邹海军基于自适应滤波的 组合导航系统华中科技大学学报（自然科学版），（）：，（），（）：，（）：，（）：，：，（）：，（）：，（）：，（）：，（）：，（）：王茂松，吴文启，何晓峰，等状态变换卡尔曼滤波的进一步解释及应用中国惯性技术学报，（）：，（）：，（）：赵仁杰，李开龙，胡柏青，等基于改进四元数阻尼误差模型的 初始对准算法系统工程与电子技术，（）：，（）：，（）：严恭敏，邓蠫传统组合导航中的实用 滤波技术评述导航定位与授时，（）：，（）：陈建华，朱海，王超，等水下 紧组合导航算法海军工程大学学报，（）：，（）：，（），（）：，（）：作者简介靳凯迪（），男，博士研究生，主要研究方向为水下无人航行器自主定位理论与方法。柴洪洲（），男，教授，博士，主要研究方向为大地测量数据处理、水下导航定位。宿楚涵（），女，助理工程师，硕士，主要研究方向为车载组合导航数据处理。向民志（），男，讲师，博士研究生，主要研究方向为海洋测绘。李明（），男，工程师，硕士研究生，主要研究方向为 组合导航算法。