上海交通大学学报

上海交通大学学报, 2024, 58(4): 498-510 doi: 10.16183/j.cnki.jsjtu.2022.513

电子信息与电气工程

基于SE(3)的鲁棒自适应算法及其在SINS/DVL中的应用

钱镭源, 覃方君, 李开龙,, 朱天高

海军工程大学 电气工程学院,武汉 430033

Robust Adaptive Algorithm Based on SE(3) and Its Application in SINS/DVL

QIAN Leiyuan, QIN Fangjun, LI Kailong,, ZHU Tiangao

School of Electrical Engineering, Naval University of Engineering, Wuhan 430033, China

通讯作者: 李开龙,讲师;E-mail:lee_kailong@163.com.

责任编辑: 王一凡

收稿日期: 2022-12-12   修回日期: 2023-01-6   接受日期: 2023-03-3  

基金资助: 国家自然科学基金(42274013)
国家自然科学基金(61873275)

Received: 2022-12-12   Revised: 2023-01-6   Accepted: 2023-03-3  

作者简介 About authors

钱镭源(1996-),博士生,从事重力辅助惯导及组合导航方面的研究.

摘要

针对复杂环境下SINS/DVL组合导航易受干扰的问题,提出了基于SE(3)的鲁棒自适应算法.通过将李群/李代数理论和鲁棒自适应策略引入正交变换容积卡尔曼滤波(TCKF),使TCKF的估计状态纳入特殊欧氏群,改善了状态空间不一致问题.并利用卡方检验和Huber方法,在量测更新时根据新息向量自适应地重构异常量测.SINS/DVL实验结果表明,所提方法具有比传统方法更优的空间一致性和鲁棒性.

关键词： 组合导航; 李群/李代数; Huber方法; 正交变换容积卡尔曼滤波

Abstract

Aimed at the problem that SINS/DVL integrated navigation is vulnerable to interference in complex environment, a robust adaptive algorithm based on SE(3) is proposed. By introducing the Lie group/Lie algebra theory and the robust adaptive strategy into the orthogonal transformed cubature Kalman filter (TCKF), the estimated states of TCKF are made to incorporate the Special Euclidean group (SE(3)), and the state space inconsistency problem is improved. The anomaly measurement is adaptively reconstructed based on the innovation vector, by using the chi-square test and the Huber method. The experimental results of SINS/DVL show that the proposed method has a better spatial consistency and robustness than the traditional methods.

Keywords： integrated navigation; Lie group/Lie algebra; Huber method; orthogonal transformed cubature Kalman filter (TCKF)

PDF (4131KB) 元数据 多维度评价 相关文章 导出 EndNote| Ris| Bibtex  收藏本文

本文引用格式

钱镭源, 覃方君, 李开龙, 朱天高. 基于SE(3)的鲁棒自适应算法及其在SINS/DVL中的应用[J]. 上海交通大学学报, 2024, 58(4): 498-510 doi:10.16183/j.cnki.jsjtu.2022.513

QIAN Leiyuan, QIN Fangjun, LI Kailong, ZHU Tiangao. Robust Adaptive Algorithm Based on SE(3) and Its Application in SINS/DVL[J]. Journal of Shanghai Jiaotong University, 2024, 58(4): 498-510 doi:10.16183/j.cnki.jsjtu.2022.513

以捷联惯性导航系统(Strapdown Inertial Navigation System,SINS)为基础,全球定位系统(Global Position System,GPS)、多普勒计程仪(Doppler Velocity Log,DVL)、超短基线(Ultra Short Base Line,USBL)等为辅的组合导航系统已广泛应用于民用、军事领域[1⇓-3].其中,SINS/DVL常用于水上导航或水下目标跟踪,是航海中常用的组合导航系统之一[4-5].对于SINS/DVL等系统,现代滤波技术是影响其导航性能的关键因素之一.卡尔曼滤波(Kalman Filter,KF)因在高斯分布条件下能得到系统的递推最小方差估计而备受关注[6-7],但却无法较好地处理非线性约束问题.由此,诸多基于KF理论的非线性滤波方法被相继提出.如扩展卡尔曼滤波(Extended Kalman Filter,EKF)[8]、无迹卡尔曼滤波(Unscented Kalman Filter,UKF)[9]、容积卡尔曼滤波(Cubature Kalman Filter,CKF)[10]、正交变换容积卡尔曼滤波(Orthogonal Transformed Cubature Kalman Filter,TCKF)[11]等.其中,TCKF对CKF的采样点进行正交变换,一定程度上解决了非局部采样问题[12].

以上传统滤波技术的状态误差被定义为理想值与实际值的差.以速度误差为例,它由理想速度值与实际计算速度值直接相加减得到.受各种误差源影响,导航系统中的实际值处于带有误差项的计算坐标系中,理想值处于理想坐标系中[13].而向量可直接加减的前提是两者处于同一坐标系.可见,传统的速度、位置等位移误差的定义方式忽略了向量方向,仅考虑了大小,易产生空间不一致问题.归根结底,这样定义位移误差的原因在于,传统欧拉角误差模型的姿态误差处于特殊正交群(Special Orthogonal Group,SO(3)),速度、位置等误差处于欧氏空间中.速度、位置和姿态状态所处空间不同,导致在定义位移误差时无法耦合姿态(方向)信息.

实际上,将刚体的旋转和平移运动之间建立一定的耦合关系,根据这一关系将导航系统中的姿态和位移状态近似定义在同一空间,即可改善误差定义的空间不一致问题,进而提升估计性能[14].随着群论的发展,李群/李代数理论使代数方程的可解性扩展到微分方程中[15],将姿态和速度等状态量引入到特殊欧氏群(Special Euclidean Group,SE(3)),使位移误差中耦合姿态信息,在提升状态估计准确性的基础上,为组合导航的状态空间一致性研究提供了新思路[16].文献[17]中推导了地球坐标系下的李群误差模型,指出该模型具有滤波一致性且更适于大失准角情况.文献[18]中对导航坐标系下SINS/GPS和SINS/DVL的SE(3)欧拉角左误差、右误差模型进行了分析,指出李群误差模型具有一定的研究必要性.文献[19-20]中基于李群仿射理论重构了EKF的状态误差方程,提出不变EKF(Invariant EKF, IEKF),使EKF的状态估计保持空间一致性.文献[21]中提出了地球坐标系下用于微惯性测量单元/全球导航卫星系统(Miniature Inertial Measurement Unit / Global Navigation Satellite System,MIMU/GNSS)的李群左误差EKF(Ln-EKF),指出Ln-EKF具有比传统滤波方法更高的精度.文献[22]中根据李群理论,提出了基于SINS/GNSS的SE2(3)左误差和右误差UKF(LSE-UKF和RSE-UKF),避免了IEKF状态转移矩阵的计算,提升了SINS/GNSS状态估计效果.SE(3)和SE2(3)均以李群/李代数理论为基础,区别在于SE2(3)较SE(3)增加了位置误差这一状态,SE(3)仅将姿态、速度状态纳入同一坐标系.可以进行旋转和平移的参数必须是矢量,速度参数在局部坐标系中满足矢量的形式.但部分研究表明,位置这一状态难以表示为“矢量直径”,同时耦合姿态误差的位置误差项在大失准角情况下会造成负面补偿[23].

同时,现代滤波技术的估计准确性也依赖于传感器提供的外部量测,而高机动、电磁干扰等复杂情况易使系统量测含有野值,导致估计性能降低.针对滤波野值问题,学者们进行了诸多有关鲁棒策略的研究.文献[24]中将最大熵准则引入交互多模型KF中,提高了KF在复杂环境下的滤波性能.文献[25]中在UKF量测更新过程中引入广义代价函数修正二次型指标,提出了基于Huber估计方法的鲁棒UKF,提高了组合导航系统在非高斯噪声环境下的鲁棒性.文献[26-27]中采用Huber-M估计,提高了CKF在导航应用中的滤波精度.

以上相关研究多针对陆地导航,而水域导航的环境更为复杂,不仅存在状态空间不一致问题,且水面起伏等诸多复杂环境造成的野值问题也较常出现.这两类问题将极大影响组合导航系统的估计准确性.为提升水域导航性能,并改进TCKF,本文在SO(3)TCKF的基础上,将姿态、速度误差等状态表示为特殊欧氏群的元素,利用李群/李代数理论和Huber方法提升其状态估计性能.首先根据李群左右误差定义,将滤波状态定义在同一坐标系中,提出了基于SE(3)的TCKF(SE(3)TCKF),以改善TCKF估计SINS/DVL状态时的空间不一致问题.其中,SE(3)TCKF是左误差TCKF(LSE-TCKF)和右误差TCKF(RSE-TCKF)的统称.同时引入卡方检验和Huber方法,使SE(3)TCKF在量测更新过程中,可根据卡方检验结果自适应地启用Huber,进而提出基于SE(3)的鲁棒自适应TCKF(SE(3)RATCKF),即LSE-RATCKF、RSE-RATCKF,进一步提高组合导航的状态估计性能.最后通过SINS/DVL实验进行综合分析,以验证所提方法的有效性.

1 基于SO(3)的TCKF

对于SINS/DVL组合导航系统,其常用的坐标系定义如下:记方向为“东-北-天”的地理坐标系为导航坐标系,即n系;方向为“右-前-上”的载体坐标系为b系;DVL的安装坐标系为d系;带有计算误差的导航坐标系为n'系;带有计算误差的载体坐标系为b'系.

SO(3)TCKF以传统的组合导航误差状态作为TCKF的估计状态,以欧拉角误差模型作为TCKF的状态和量测方程,是组合导航模型与TCKF的结合.根据文献[28]中SO(3)UKF的推导过程,可得SO(3)TCKF的具体步骤,即参数初始化、计算采样点、状态更新、量测更新、状态估计和反馈校正.

(1) 参数初始化.

对SO(3)TCKF的状态向量、协方差矩阵、噪声矩阵等参数进行初始化.

(2) 计算采样点.

TCKF在CKF基础上,对Sigma点进行正交变换,得到一组新的满足三阶精度数值积分的采样点.这一步骤是TCKF的基础,属于常见公式,在有关于TCKF的研究中均有介绍,具体计算步骤见文献[11-12],本文在此不做赘述.

(3) 状态更新.

在得到TCKF的采样点后,即可计算状态更新过程中的估计采样点:

Pk-1+=Sk-1+(Sk-1+)T
$\chi _{j}^{+}=S_{\text{k}-1}^{+}{{\lambda }_{j}}+{{x}_{k}}_{-1}$

式中:Pk-1+为k-1时刻的状态协方差矩阵;Sk-1+为Pk-1+经Cholesky分解后得到的矩阵;λj为TCKF的采样点,j=1, 2, …, 2n,n为状态维数;xk-1为k-1时刻SINS/DVL的状态向量.xk=[δφE δφN δφU δvE δvN δvU δL δλ δh Δx Δy Δz εx εy εz]T,δφE、δφN和δφU为欧拉角误差,δvE、δvN和δvU为“东-北-天”三轴速度误差,δL、δλ和δh为“经纬高”位置误差,Δx、Δy、Δz和εx、εy、εz分别为陀螺仪、加速度计的三轴测量误差.

根据式(2)计算一步状态预测:

${{\bar{x}}_{\text{k}}}=\overset{2n}{\mathop{\underset{j=1}{\mathop \sum }\,}}\,{{\omega }_{j}}f(\chi _{j}^{+})$

式中:f(·)为状态转移方程,具体指SINS/DVL的状态更新方程;ωj为TCKF的权值,ωj=1/(2n).为便于推导,记χj+在每j次计算中的形式为χj+=[dφ dv dp dΔ dε]T.其中,各状态的采样点可表示为

dφ=[dφE dφN dφU]T

dv=[dvE dvN dvU]T

dp=[dL dλ dh]T

dΔ=[dΔx dΔy dΔz]T

dε=[dεx dεy dεz]T

由SINS/DVL欧拉角误差更新方程[29]可知:

式中:(·)×为·的反对称矩阵;~表示带有计算误差的导航参数;ωinn=ωien+ωenn表示n系下的载体角速度,ωien为n系下地球自转角速度,ωenn为牵连角速度;Cbn为b系到n系的方向余弦矩阵;vn=vEvNvUT为n系下的载体速度;T为采样周期.

δωien=0-ωiesinLdLωiecosLdLδωenn=-dvN/(RM+h)MmcosL-NnsinLdLMmsinL+NncosLdL
Mm=dvE+v~EtanLdL(RN+h)cosL, Nn=v~E(RN+h)cosL
M1=01RM+h01(RN+h)cosL00001M2=000v~NtanL(RN+h)cosL00000

式中:ωie为地球自转角速度;RN和RM分别为地球卯酉圈和子午圈的曲率半径.

根据式(3),得到一步状态预测的协方差矩阵:

$P_{k}^{-}=\overset{2n}{\mathop{\underset{j=1}{\mathop \sum }\,}}\,{{\omega }_{j}}(f(\chi _{j}^{+})-{{\bar{x}}_{k}}){{(f(\chi _{j}^{+})-{{\bar{x}}_{k}})}^{T}}+{{Q}_{k}}$

式中:Qk为状态噪声协方差矩阵.

(4) 量测更新.

进行量测更新过程中的估计采样点计算:

Pk-=Sk-(Sk-)T
χj-=Sk-λj+ x-k

式中:Sk-为Pk-经Cholesky分解后得到的矩阵.

根据式(10),计算量测预测:

${{\bar{z}}_{k}}=\overset{2n}{\mathop{\underset{j=1}{\mathop \sum }\,}}\,(H\chi _{j}^{-}){{\omega }_{j}}$

式中:H= -(v~DVLn)×I3×303×9为量测矩阵,v~DVLn由DVL实际输出的速度v~DVLd经旋转变换得到.

进而得到对应的量测预测协方差矩阵:

$P_{k}^{\text{zz}}=\overset{2n}{\mathop{\underset{j=1}{\mathop \sum }\,}}\,(H\chi _{j}^{-}-{{\bar{z}}_{k}}){{(H\chi _{j}^{-}-{{\bar{z}}_{k}})}^{T}}{{\omega }_{j}}+{{R}_{k}}$

式中:Rk为量测噪声协方差矩阵.

(5) 状态估计.

SO(3)TCKF的状态估计包括互协方差矩阵、卡尔曼滤波增益、状态估计更新、协方差矩阵更新的计算.以上步骤的计算公式与CKF、TCKF相同,为非线性卡尔曼滤波的常见公式,具体可见文献[11-12],本文在此不做赘述.

(6) 反馈校正.

SO(3)TCKF的反馈校正是对姿态、速度和位置进行误差补偿:

Cb, udn=exp(δφ)C~bnvudn=v~n-δvpud=p~-δp

式中:δφ= δφEδφNδφUT,δv=[δvE δvN δvU]T,δp=[δL δλ δh]T.

由式(13)可知,速度误差被定义为δv=vn-v~n,实则vn属于n系,v~n属于n'系,直接相减并不严谨,因此还应考虑两系之间方向的差异.这一问题因SO(3)TCKF的姿态、位移状态所在空间不同而难以解决.

2 基于SE(3)的TCKF

为提高SO(3)TCKF的空间一致性,根据李群/李代数理论,重新定义误差状态,改写状态、量测预测和反馈校正,使SINS/DVL组合导航的状态纳入SE(3)空间,提出了基于SE(3)的TCKF.

2.1 理论基础

本节进行SE(3)TCKF理论基础的讨论,主要包括李群/李代数理论及SINS/DVL状态的SE(3)表示.

李群是指具有连续性质的群,每一个群与一个李代数一一对应.

SO(3)仅可描述刚体的旋转运动,SE(3)则包含平移和旋转,两者分别由以下公式表示:

$SO(3)=r\in SO(3)|r{{r}^{T}}={{I}_{3\times 3}},\left| r \right|=1$
SE(3)=T=rt01∈R4×4r∈R3×3,t∈R3

式中:r和t分别表示旋转矩阵和平移向量;R4×4表示4×4矩阵.

由式(14)和(15)可知,SE(3)较SO(3)能更为准确地描述刚体运动,其所对应的李代数为

$se(3)=\left\{ \zeta =\left[ \begin{array}{*{35}{l}} \phi \\ \xi \\ \end{array} \right]\in {{R}^{6}},\phi \in so(3),\xi \in {{R}^{3}},{{(\zeta )}_{\times }}=\left[ \begin{array}{*{35}{l}} {{\left( \phi \right)}_{\times }} & \xi \\ 0 & 1 \\ \end{array} \right]\in {{R}^{4\times 4}} \right\}$

式中:ϕ和ξ分别表示旋转、平移矢量;ζ为由ϕ和ξ组成的6维矢量;so(3)为SO(3)对应的李代数.

SE(3)和se(3)存在如下关系:

exp((ζ)×)=exp((ϕ)×)Jv01×31

式中:Jv表示速度误差.当ϕ取极小值时,exp((ϕ)×)≈I3×3+(ϕ)×,J可近似为单位矩阵.

根据李群和李代数,将Cbn和vn纳入SE(3)中,以证明SINS/DVL的状态满足群仿射性质.

χ=Cbnvn01×31∈SE(3)

对上式求逆:

χ-1=Cnb-Cnbvn01×31∈SE(3)

χ的微分方程可表示为

$\begin{matrix} & \dot{\chi }=f\left( \chi \right)=\left[ \begin{array}{*{35}{l}} C_{\text{b}}^{\text{n}}{{(\omega _{\text{ib}}^{\text{b}})}_{\times }}-{{(\omega _{\text{in}}^{\text{n}})}_{\times }}C_{\text{b}}^{\text{n}} & C_{\text{b}}^{\text{n}}f_{\text{ib}}^{\text{b}}-{{(2\omega _{\text{ie}}^{\text{n}}+\omega _{\text{en}}^{\text{n}})}_{\times }}{{v}^{\text{n}}}+{{g}^{\text{n}}} \\ {{0}_{1\times 3}} & 1 \\ \end{array} \right] \\ & =\left[ \begin{array}{*{35}{l}} C_{\text{b}}^{\text{n}} & {{v}^{\text{n}}} \\ {{0}_{1\times 3}} & 1 \\ \end{array} \right]\left[ \begin{array}{*{35}{l}} {{(\omega _{\text{ib}}^{\text{b}})}_{\times }} & f_{\text{ib}}^{\text{b}} \\ {{0}_{1\times 3}} & 0 \\ \end{array} \right]+\left[ \begin{array}{*{35}{l}} {{(\omega _{\text{in}}^{\text{n}})}_{\times }} & {{g}^{\text{n}}}-{{(\omega _{\text{ie}}^{\text{n}})}_{\times }}{{v}^{\text{n}}} \\ {{0}_{1\times 3}} & 0 \\ \end{array} \right]\left[ \begin{array}{*{35}{l}} C_{\text{b}}^{\text{n}} & {{v}^{\text{n}}} \\ {{0}_{1\times 3}} & 1 \\ \end{array} \right] \\ & \text{=}\chi {{W}_{1}}+{{W}_{2}}\chi \\ \end{matrix}$

令χ1=χ2=χ,可证明导航状态是满足群仿射性质的[20],即

$f({{\chi }_{1}}{{\chi }_{2}})={{\chi }_{1}}{{\chi }_{2}}{{W}_{1}}+{{W}_{2}}{{\chi }_{1}}{{\chi }_{2}}=f({{\chi }_{1}}){{\chi }_{2}}+{{\chi }_{1}}f({{\chi }_{2}})-{{\chi }_{1}}f({{I}_{5\times 5}}){{\chi }_{2}}$

因此,SINS/DVL的状态满足不变特性,可对其进行重新定义.

2.2 左误差SE(3)TCKF

根据误差定义的不同,误差状态可分为左误差和右误差两种[17,20],即τL=χ~-1χ和τR=χχ~-1.本节进行左误差SE(3)TCKF(LSE-TCKF)的推导.

设LSE-TCKF估计的SINS/DVL状态为:xL, k=[δφL δvL δpL δΔ δε]T.

根据2.1节,可以得到重新定义后的满足群不变特性的左误差状态:

${{\tau }_{L}}={{\tilde{\chi }}^{-1}}\chi =\left[ \begin{array}{*{35}{l}} \tilde{C}_{\text{n}}^{\text{b}}C_{\text{b}}^{\text{n}} & \tilde{C}_{\text{n}}^{\text{b}}\left( {{v}^{\text{n}}}-{{{\tilde{v}}}^{\text{n}}} \right) \\ {{0}_{1\times 3}} & 1 \\ \end{array} \right]=\left[ \begin{array}{*{35}{l}} \tau _{\text{L}}^{ \alpha } & \tau _{\text{L}}^{\text{v}} \\ {{0}_{1\times 3}} & 1 \\ \end{array} \right]\in SE(3)$

式中:τLα和τLv分别为新定义的左不变姿态误差和速度误差;τLα=exp((φL)×)=C~nbCbn=Cbb';τLv=C~nb(vn-v~n)=-Cnb'δvn.由于误差定义不同,LSE-TCKF与SO(3)TCKF的不同之处在于与状态误差有关的状态预测、量测预测和反馈校正.

首先进行LSE-TCKF状态预测的推导.

结合式(22),对τLα、τLv求微分[30],即可得到姿态和速度的误差微分方程:

$\begin{matrix} & \frac{d}{\text{d}t}\tau _{\text{L}}^{ \alpha }=\frac{\text{d}}{\text{d}t}\tilde{C}_{\text{n}}^{\text{b}}C_{\text{b}}^{\text{n}}=\frac{\text{d}}{\text{d}t}C_{\text{n}}^{\text{b}'}C_{\text{b}}^{\text{n}}=\frac{\text{d}}{\text{d}t}C_{\text{b}'}^{\text{b}}\approx \\ & \frac{\text{d}}{\text{d}t}({{(\delta {{\varphi }_{L}})}_{\times }})=-{{[{{(\tilde{\omega }_{\text{ib}}^{\text{b}})}_{\times }}\delta {{\varphi }_{L}}]}_{\times }}-{{(\delta \omega _{\text{ib}}^{\text{b}})}_{\times }}+{{(C_{\text{n}}^{\text{b}'}\delta \omega _{\text{in}}^{\text{n}})}_{\times }} \\ \end{matrix}$
$\frac{\text{d}}{\text{d}t}\delta {{\varphi }_{L}}=-{{(\tilde{\omega }_{\text{ib}}^{\text{b}})}_{\times }}\delta {{\varphi }_{L}}-\delta \omega _{\text{ib}}^{\text{b}}+C_{\text{n}}^{\text{b}'}\delta \omega _{\text{in}}^{\text{n}}$
$\begin{matrix} & \frac{\text{d}}{\text{d}t}\tau _{\text{L}}^{\text{v}}=\frac{\text{d}}{\text{d}t}C_{\text{n}}^{\text{b}'}({{v}^{n}}-{{{\tilde{v}}}^{\text{n}}})= \\ & -{{[{{(C_{\text{n}}^{\text{b}'})}^{T}}\tilde{\omega }_{\text{ie}}^{\text{n}}]}_{\times }}\tau _{\text{L}}^{\text{v}}-{{(\tilde{\omega }_{\text{ib}}^{\text{b}})}_{\times }}\tau _{\text{L}}^{\text{v}}+ \\ & (C_{\text{b}'}^{\text{b}}-{{I}_{3\times 3}})\tilde{f}_{\text{ib}}^{\text{b}}-\delta f_{\text{ib}}^{\text{b}}+ \\ & {{(C_{\text{n}}^{\text{b}'})}^{T}}{{(2\delta \omega _{\text{ie}}^{\text{n}}+\delta \omega _{\text{en}}^{\text{n}})}_{\times }}{{{\tilde{v}}}^{\text{n}}} \\ \end{matrix}$

式中:fibb表示比力;ωibb为陀螺仪输出.

由于速度误差定义不同,左误差SE(3)中的位置误差δpL的微分方程为

$\frac{d}{\text{d}t}\delta {{p}_{L}}={{N}_{rr}}\delta {{p}_{L}}+{{N}_{rv}}(\delta {{v}^{n}})={{N}_{rr}}\delta {{p}_{L}}+{{N}_{rv}}(-C_{\text{b}'}^{\text{n}}\tau _{\text{L}}^{\text{v}})$

式中:

Nrr=00-v~N(RM+h)2v~EtanL(RN+h)cosL0-v~E(RN+h)2cosL000,Nrv=M1.

式(24)~(26)称为SE(3)左误差微分方程.进而可得LSE-TCKF的一步状态预测如下:

${{\bar{x}}_{k}}=\overset{2n}{\mathop{\underset{j=1}{\mathop \sum }\,}}\,{{\omega }_{j}}{{f}_{LSE}}(\chi _{\text{j}}^{+})$

式中:fLSE(χj+)为状态转移方程,

dφL、dvL、dpL对应式(4)中的dφ、dv、dp.

然后进行LSE-TCKF量测预测的推导.

根据左误差定义,LSE-TCKF的量测方程为

$z_{\text{M}}^{\text{v}}=\delta {{v}^{n}}={{\tilde{v}}^{\text{n}}}-{{v}^{n}}=-C_{\text{b}'}^{\text{n}}\tau _{\text{L}}^{\text{v}}$

式中:zMv为SINS/DVL的量测值,为SINS和DVL提供的速度之差.

进而可得其量测预测为

${{\bar{z}}_{\text{k}}}=\overset{2n}{\mathop{\underset{j=1}{\mathop \sum }\,}}\,~(-C_{\text{b}'}^{\text{n}}\chi _{j,\text{v}}^{-}){{\omega }_{j}}$

式中:χj,v-为每j次运算时χj-中的速度状态.

根据左误差定义,可得LSE-TCKF的反馈校正如下:

Cb, Ludn=Cb'nexp(δφL)vLudn=v~n-δvn=v~n+Cb'nτLvpLud=p~-δpL

将SO(3)TCKF中的式(3)、(11)、(13)替换为式(27)、(30)、(31),即可得到基于左误差SE(3)的TCKF——LSE-TCKF.

2.3 右误差SE(3)TCKF

本节进行右误差SE(3)TCKF(RSE-TCKF)的推导.

设右误差RSE-TCKF所估计的SINS/DVL状态为:xR, k=[δφR δvR δpR δΔ δε]T.

根据式(18)和(19),可得到重新定义后的满足群不变特性的右误差状态:

${{\tau }_{R}}=\chi {{\tilde{\chi }}^{-1}}=\left[ \begin{array}{*{35}{l}} C_{\text{b}}^{\text{n}}\tilde{C}_{\text{n}}^{\text{b}} & {{v}^{\text{n}}}-C_{\text{b}}^{\text{n}}\tilde{C}_{\text{n}}^{\text{b}}{{{\tilde{v}}}^{\text{n}}} \\ {{0}_{1\times 3}} & 1 \\ \end{array} \right]=\left[ \begin{array}{*{35}{l}} \tau _{\text{R}}^{ \alpha } & \tau _{\text{R}}^{\text{v}} \\ {{0}_{1\times 3}} & 1 \\ \end{array} \right]\in \text{SE}\left( 3 \right)$

式中:τRα和τRv分别为新定义的右不变姿态误差和速度误差,τRα=exp((φR)×)=CbnC~nb=Cn'n,τRv=vn-CbnC~nbv~n=vn-Cn'nv~n=v~n-Cn'nv~n-δvn.由于误差定义不同,与LSE-TCKF一样,RSE-TCKF与SO(3)TCKF的不同之处也在于状态预测、量测预测和反馈校正.

首先进行RSE-TCKF状态预测的推导.

对τRα、τRv求微分[30],可得右误差SE(3)的姿态、速度误差微分为

$\begin{matrix} & \frac{d}{\text{d}t}\tau _{\text{R}}^{ \alpha }=\frac{\text{d}}{\text{d}t}C_{\text{b}}^{\text{n}}\tilde{C}_{\text{n}}^{\text{b}}=\frac{\text{d}}{\text{d}t}C_{\text{b}}^{\text{n}}C_{\text{n}'}^{\text{b}}=\frac{\text{d}}{\text{d}t}C_{\text{n}'}^{\text{n}}\approx \\ & \frac{\text{d}}{\text{d}t}~({{(\delta {{\varphi }_{R}})}_{\times }})=-{{[{{(\tilde{\omega }_{\text{in}}^{\text{n}})}_{\times }}\delta {{\varphi }_{R}}]}_{\times }}+ \\ & {{(\delta \omega _{\text{in}}^{\text{n}})}_{\times }}-{{(\tilde{C}_{\text{b}}^{\text{n}}\delta \omega _{\text{ib}}^{\text{b}})}_{\times }} \\ \end{matrix}$
$\frac{\text{d}}{\text{d}t}\delta {{\varphi }_{R}}=-{{(\tilde{\omega }_{\text{in}}^{\text{n}})}_{\times }}\delta {{\varphi }_{R}}+\delta \omega _{\text{in}}^{\text{n}}-C_{\text{n}'}^{\text{b}}\delta \omega _{\text{ib}}^{\text{b}}$
$\begin{matrix} & \frac{\text{d}}{\text{d}t}\tau _{\text{R}}^{\text{v}}=\frac{\text{d}}{\text{d}t}({{v}^{n}}-C_{\text{b}}^{\text{n}}C_{\text{n}'}^{\text{b}}{{{\tilde{v}}}^{\text{n}}})= \\ & -C_{\text{n}'}^{\text{n}}C_{\text{n}'}^{\text{b}}\delta f_{\text{ib}}^{\text{b}}-[{{(2\tilde{\omega }_{\text{ie}}^{\text{n}}+\tilde{\omega }_{\text{en}}^{\text{n}})}_{\times }}]\tau _{\text{R}}^{\text{v}}- \\ & {{(C_{\text{n}'}^{\text{n}}{{{\tilde{v}}}^{\text{n}}})}_{\times }}({{I}_{3\times 3}}-{{(C_{\text{n}'}^{\text{n}})}^{T}})\tilde{\omega }_{\text{ie}}^{\text{n}}- \\ & {{(C_{\text{n}'}^{\text{n}}{{{\tilde{v}}}^{\text{n}}})}_{\times }}\delta \omega _{\text{ie}}^{\text{n}}+({{I}_{3\times 3}}-C_{\text{n}'}^{\text{n}}){{{\tilde{g}}}^{\text{n}}}- \\ & {{(C_{\text{n}'}^{\text{n}}{{{\tilde{v}}}^{\text{n}}})}_{\times }}C_{\text{n}'}^{\text{b}}\delta \omega _{\text{ib}}^{\text{b}} \\ \end{matrix}$

式中:gn为重力加速度.

由于速度的误差定义不同,右误差SE(3)中的位置误差δpR的微分方程为

$\frac{d}{\text{d}t}\delta {{p}_{R}}={{N}_{rr}}\delta {{p}_{R}}+{{N}_{rv}}\delta {{v}_{R}}={{N}_{rr}}\delta {{p}_{R}}+{{N}_{rv}}({{\tilde{v}}^{\text{n}}}-C_{\text{n}'}^{\text{n}}{{\tilde{v}}^{\text{n}}}\tau _{\text{R}}^{\text{v}})$

根据式(34)、(35)、(36),可得RSE-TCKF的一步状态预测如下:

${{\bar{x}}_{k}}=\overset{2n}{\mathop{\underset{j=1}{\mathop \sum }\,}}\,{{\omega }_{j}}{{f}_{RSE}}(\chi _{j}^{+})$

式中:fRSE(χj+)为状态转移方程,

dφR、dvR、dpR对应式(4)中的dφ、dv、dp.

然后进行RSE-TCKF量测预测的推导.

根据右误差定义,RSE-TCKF的量测方程为

$z_{\text{M}}^{\text{v}}=\delta {{v}^{n}}={{\tilde{v}}^{\text{n}}}-{{v}^{n}}={{\tilde{v}}^{\text{n}}}-C_{\text{n}'}^{\text{n}}{{\tilde{v}}^{\text{n}}}-\tau _{\text{R}}^{\text{v}}$

进而可得其量测预测如下:

${{\bar{z}}_{k}}=\overset{2n}{\mathop{\underset{j=1}{\mathop \sum }\,}}\,({{\tilde{v}}^{\text{n}}}-C_{\text{n}'}^{\text{n}}{{\tilde{v}}^{\text{n}}}-\chi _{j, ~\!\!\text{ v}}^{-}){{\omega }_{j}}$

根据右误差定义,可得RSE-TCKF的反馈校正如下:

Cb, Rudn=exp(δφR)Cbn'vRudn=v~n-δvn=v~n-Cn'nv~n+τRvpRud=p~-δpL

将SO(3)TCKF中的式(3)、(11)、(13)替换为式(37)、(40)、(41),即可得到基于右误差SE(3)的TCKF——RSE-TCKF.

3 基于SE(3)的鲁棒自适应TCKF

SE(3)TCKF经状态误差的重新定义,改善了SO(3)TCKF的空间不一致问题,但仍然容易受外界复杂环境的干扰而产生量测野值,导致估计精度下降.Huber方法通过构造非线性回归方程,建立广义代价函数来重构量测相关信息,以避免异常值的影响,是一种常用的鲁棒策略[31].为进一步提升SE(3)TCKF 的鲁棒性,保证其具有较高的状态估计性能,本节根据上一节左误差和右误差SE(3)TCKF的量测预测,构造不同的Huber非线性回归方程,利用卡方检验和Huber方法对SE(3)TCKF的量测值进行自适应重构,提出了基于SE(3)左误差和右误差的鲁棒自适应TCKF(SE(3)RATCKF).SE(3)RATCKF在量测更新部分,采用卡方检验实时检测量测新息,当超出卡方检验阈值时,认为量测异常,自适应地启用Huber方法重构异常量测;否则不进行鲁棒处理.SE(3)RATCKF的具体步骤如图1所示.

图1

新窗口打开| 下载原图ZIP| 生成PPT

图1   SE(3)RATCKF具体步骤

Fig.1   Specific steps of SE(3)RATCKF


SE(3)RATCKF的鲁棒自适应改进如下.

(1) 在SE(3)TCKF的滤波增益计算完成后,根据SINS/DVL的量测及式(30)或(40)构造新息序列和卡方检验统计量:

${{\varepsilon }_{k}}=z_{\text{M}}^{\text{v}}-{{\bar{z}}_{k}}$
${{G}_{k}}={{\varepsilon }^{\text{T}}}_{k}{{(P_{k}^{zz})}^{-1}}{{\varepsilon }_{k}}$

式中:Gk服从自由度为d的卡方分布,d=3为量测维数.当量测信息无异常时,εk是均值为0的高斯白噪声;否则εk会产生偏差,均值不再为0.定义量测信息异常的判定准则:

$\left.\begin{array}{l}\left|\boldsymbol{G}_k\right|>T_{\mathrm{D}} \quad \text { 量测信息异常 } \\ \left|\boldsymbol{G}_k\right| \leqslant T_{\mathrm{D}} \Rightarrow \text { 量测信息正常 } \end{array} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \right\}$

式中:TD为预先设置的故障门限值,其取值可对应某个误警率pf.由卡方分布表可得到TD和pf的对应关系,误警率pf越小,其故障诊断能力越高.

(2) 当量测信息正常时,采用SO(3)TCKF步骤(5)中的状态估计更新和协方差更新公式进行计算.

(3) 当量测信息异常时,进行Huber运算.

首先,构造左右误差SE(3)RATCKF的非线性回归方程.其中,LSE-RATCKF的非线性回归方程如下:

zMvx-k=hL(xv, k)xk+vk-δx-k

式中:δ x-k=xk-x-k;hL(xv,k)=-Cb'nxv,k;xv,k为xk的速度状态;vk为量测噪声.

相应地,RSE-RATCKF的非线性回归方程如下:

zMvx-k=hR(xv,k)xk+vk-δx-k

式中:hR(xv,k)=v~n-Cn'nv~nxv,k.

然后,定义以下变量:

Dk=Rk00Pk-
yk=Dk-1/2zkx-k
g(xk)=Dk-1/2h(xv,k)xk
μk=Dk-1/2vk-δx-k

当SINS/DVL采用左误差定义时,h(xv,k)=hL(xv,k);当采用右误差定义时,h(xv,k)=hR(xv,k).

得到重构后的量测信息:

$\tilde{z}_{\text{M}}^{\text{v}}=D_{k}^{1/2}{{\tilde{y}}_{k}}~(1:c)=D_{k}^{1/2}~[g({{x}_{k}})+\Psi (1:c,1:c){{e}_{k}}(1:c)]$

式中:c为状态与量测的维数之和;y~k=g(xk)+e~k为修正后的yk;e　~k为根据Huber方法修正后的新息序列[32].

进而进行量测异常时的状态估计更新:

${{\tilde{x}}_{k}}={{\bar{x}}_{k}}+{{K}_{k}}(\tilde{z}_{\text{M}}^{\text{v}}-{{\bar{z}}_{k}})$

最后,进行量测异常时的协方差矩阵更新.

由SE(3)RATCKF的鲁棒自适应改进过程可知,卡方检验使Huber方法仅在量测异常时启动,避免了Huber在量测信息正常时也进行的zMv=z~Mv替代.

4 SINS/DVL实验验证

为了验证SE(3)RATCKF的性能,分别进行了空间一致性及鲁棒自适应策略验证实验.实验过程中,以某公司生产的IMU和DVL作为实验对象,以一台高精度GPS和IMU组成的组合导航系统作为基准,于湖北省巴东县长江进行船载SINS/DVL组合导航.其中,IMU和DVL设备的数据更新频率分别为200和1 Hz,其余主要参数如表1所示,行驶过程中载体轨迹和DVL提供的量测信息(速度)分别如图2和图3所示.由图3可知,载体因水面起伏等复杂环境,使DVL测得带有野值的速度信息.

表1   SINS/DVL组合导航船载实验传感器参数

Tab.1  Parameters of SINS/DVL integrated navigation shipboard test sensor

传感器误差项误差值
IMU加速度计漂移 (10-6g)≤50
IMU陀螺仪漂移/ [(°)·h-1]≤0.02
DVL位置误差/ m≤10
DVL速度误差/ (m·s-1)≤0.005

新窗口打开| 下载CSV


图2

新窗口打开| 下载原图ZIP| 生成PPT

图2   载体轨迹

Fig.2   Trajectory of carrier


图3

新窗口打开| 下载原图ZIP| 生成PPT

图3   DVL提供的量测信息

Fig.3   Measurement information provided by DVL


4.1 SE(3)TCKF空间一致性实验

失准角的增大会加剧理想坐标系和计算坐标系之间的不一致性,本节通过对比不同失准角下SO(3)TCKF、LSE-TCKF、RSE-TCKF的状态估计效果,以检验SE(3)TCKF的状态空间一致性.分别设置初始失准角为(0°, 0°, 0°)、(5°, 5°, 10°)、(25°,25°, 25°)、(30°, 30°, 45°),姿态、速度、位置误差由下式计算:

φerr=(θerr)2+(γerr)2+(ψerr)2
verr=(verrE)2+(verrN)2+(verrN)2
perr=(Lerr)2+(λerr)2

式中:θerr、γerr、ψerr分别为俯仰角、横滚角和航向角误差;verrE、verrN、verrU分别为东北天3向速度误差;Lerr、λerr分别为纬度和经度误差.3种方法的导航误差如图4~7所示,对应的均方根误差(RMSE)如表2~5所示,失准角变化时LSE-TCKF和RSE-TCKF较SO(3)TCKF精度提升比例如表6所示.

图4

新窗口打开| 下载原图ZIP| 生成PPT

图4   失准角为(0°, 0°, 0°)时的导航误差

Fig.4   Navigation error at a misalignment angle of (0°, 0°, 0°)


图5

新窗口打开| 下载原图ZIP| 生成PPT

图5   失准角为(5°, 5°, 10°)时的导航误差

Fig.5   Navigation error at a misalignment angle of (5°, 5°, 10°)


图6

新窗口打开| 下载原图ZIP| 生成PPT

图6   失准角为(25°, 25°, 25°)时的导航误差

Fig.6   Navigation error at a misalignment angle of (25°, 25°, 25°)


图7

新窗口打开| 下载原图ZIP| 生成PPT

图7   失准角为(30°, 30°, 45°)时的导航误差

Fig.7   Navigation error at a misalignment angle of (30°, 30°, 45°)


表2   失准角为(0°, 0°, 0°)时的RMSE

Tab.2  RMSE at a misalignment angle of (0°, 0°, 0°)

类别姿态误差/(°)速度误差/
(m·s-1)		位置误差/m
SO(3)TCKF0.439 80.270 1148.147 7
LSE-TCKF0.406 30.272 6145.222 0
RSE-TCKF0.489 80.270 5148.556 2

新窗口打开| 下载CSV


表3   失准角为(5°, 5°, 10°)时的RMSE

Tab.3  RMSE at a misalignment angle of (5°, 5°, 10°)

类别姿态误差/(°)速度误差/
(m·s-1)		位置误差/m
SO(3)TCKF5.721 60.282 3168.388 0
LSE-TCKF2.886 40.280 0130.768 1
RSE-TCKF3.762 90.291 6112.626 6

新窗口打开| 下载CSV


表4   失准角为(25°, 25°, 25°)时的RMSE

Tab.4  RMSE at a misalignment angle of (25°, 25°, 25°)

类别姿态误差/(°)速度误差/
(m·s-1)		位置误差/m
SO(3)TCKF13.796 00.825 2267.466 8
LSE-TCKF4.441 30.294 8124.607 7
RSE-TCKF8.349 50.461 983.175 3

新窗口打开| 下载CSV


表5   失准角为(30°, 30°, 45°)时的RMSE

Tab.5  RMSE at a misalignment angle of (30°, 30°, 45°)

类别姿态误差/(°)速度误差/
(m·s-1)		位置误差/m
SO(3)TCKF28.539 22.945 7530.985 0
LSE-TCKF8.267 50.368 1111.966 1
RSE-TCKF18.539 61.976 3291.014 9

新窗口打开| 下载CSV


表6   失准角变化时LSE-TCKF和RSE-TCKF较SO(3)TCKF精度提升比例

Tab.6  Percentage improvement in accuracy of LSE-TCKF and RSE-TCKF compared to SO(3)TCKF at variations in misalignment angle

失准角算法姿态精度提升比例/%速度精度提升比例/%位置精度提升比例/%
(5°, 5°, 10°)LSE-TCKF49.55-3.2922.34
(5°, 5°, 10°)RSE-TCKF34.230.8133.11
(25°, 25°, 25°)LSE-TCKF67.8164.2853.41
(25°, 25°, 25°)RSE-TCKF39.4844.0368.91
(30°, 30°, 45°)LSE-TCKF71.0387.5178.91
(30°, 30°, 45°)RSE-TCKF35.0432.9145.19

新窗口打开| 下载CSV


由图4和表2可知,当失准角为(0°, 0°, 0°)时,SO(3)TCKF、LSE-TCKF、RSE-TCKF的姿态、速度、位置误差的差异较小.这是因为失准角较小时,SO(3)TCKF坐标系的方向不一致性并不明显.由图5~7和表3~5可知,当失准角逐渐增大时,理想坐标系和计算坐标系方向差异越发明显,SO(3)TCKF的估计效果也逐渐降低;而LSE-TCKF和RSE-TCKF将系统状态纳入同一坐标系,均改善了SO(3)TCKF的空间不一致性.由表6可知,随着失准角的增大,LSE-TCKF和RSE-TCKF的改进优势逐渐明显,LSE-TCKF相对较优.RSE-TCKF的改进效果在失准角为(5°, 5°, 10°)、(25°, 25°, 25°)时提升,而在(30°, 30°, 45°)时下降是因为,DVL提供的速度是一个弱观测值,易受到其他信息的干扰,如陀螺仪和加速度计偏差、风、浪、水流等.所以,“不变观测”[16]理论在某些情况下并不适合SINS/DVL.综合不同失准角下导航参数的估计结果,可知SE(3)TCKF有效解决了SO(3)TCKF的空间不一致问题.

4.2 鲁棒自适应策略验证实验

为了验证鲁棒自适应改进的有效性,设置失准角不变,即(0°, 0°, 0°),以保证导航性能仅受异常量测(野值)影响.分别采用SE(3)TCKF、SE(3)HTCKF(基于Huber方法的SE(3)TCKF)、SE(3)MCCTCKF(基于最大熵准则的SE(3)TCKF)、SE(3)RATCKF估计载体的姿态、速度和位置.其中,SE(3)HTCKF是LSE-HTCKF和RSE-HTCKF的统称,SE(3)MCCTCKF是LSE-MCCTCKF和RSE-MCCTCKF的统称.不同改进策略的导航误差如图8所示,对应的RMSE如表7所示.

图8

新窗口打开| 下载原图ZIP| 生成PPT

图8   不同改进策略的导航误差比较

Fig.8   Comparison of navigation errors of different improvement strategies


表7   不同改进策略的RMSE

Tab.7  RMSE of different improvement strategies

类别姿态误差/(°)速度误差/
(m·s-1)		位置误差/m
LSE-HTCKF0.395 90.077 654.716 3
RSE-HTCKF0.454 40.074 153.266 1
LSE-MCCTCKF0.401 00.073 358.292 8
RSE-MCCTCKF0.519 30.091 859.957 5
LSE-RATCKF0.388 50.071 348.036 3
RSE-RATCKF0.474 80.071 146.755 7

新窗口打开| 下载CSV


由图4、图8、表2和表7可知,SO(3)TCKF和SE(3)TCKF因未引入鲁棒策略,所以易受量测野值的影响,估计误差较大;SE(3)HTCKF、SE(3)MCCTCKF和SE(3)RATCKF因引入Huber方法或最大相关熵准则而具有较强的鲁棒性.通过对比图8和表7各方法的估计误差,可以看出SE(3)RATCKF因卡方检验,仅在量测信息异常时自适应地启动Huber估计, 避免了SE(3)HTCKF在量测正常时仍进行重构而造成的过量补偿,对LSE-TCKF和RSE-TCKF的改进效果最优,Huber方法次之,最大熵准则最低.因此,本文的鲁棒自适应策略能够有效解决SINS/DVL量测野值问题,进一步提升SE(3)TCKF的估计精度和鲁棒性.

5 结语

本文根据李群理论,将SO(3)TCKF估计的姿态误差、速度误差等状态纳入同一坐标系中,求得SE(3)空间中的左右误差状态、量测更新方程和反馈校正,提出了基于SE(3)的TCKF,提升了SO(3)TCKF的空间一致性.并针对SINS/DVL容易引入异常量测的问题,将卡方检验和Huber方法引入SE(3)TCKF中,通过卡方检验检测SE(3)TCKF的量测新息,自适应地判断是否启用Huber,以提升SE(3)TCKF的鲁棒性,进而提出带有鲁棒自适应策略的SE(3)TCKF即SE(3)RATCKF.SINS/DVL实验结果表明,所提方法可有效解决SO(3)TCKF的状态空间不一致及量测异常问题,避免了常规Huber方法在量测正常时的过量重构,具有较高的鲁棒性和估计准确性.后续可对该算法的实用性进行深入研究.

参考文献

高红莲, 尤杰, 曹松银.

基于PF-UKF组合滤波的SINS/GPS组合导航系统空中对准方法

[J]. 上海交通大学学报, 2022, 56(11): 1447-1452.

DOI:10.16183/j.cnki.jsjtu.2022.167      [本文引用: 1]

针对捷联式惯性导航系统(SINS)/全球定位系统(GPS)组合导航系统模型的误差以及粒子滤波(PF)存在的粒子退化问题,结合无迹卡尔曼滤波(UKF)算法,提出一种基于PF-UKF组合滤波的SINS/GPS组合导航系统空中对准方法.由误差四元数代替姿态角,以SINS和GPS的位置差和速度差作为观测量,建立新的组合导航系统误差方程.所提出的PF-UKF组合滤波算法将采样粒子分为随机粒子和确定粒子,其中随机粒子为概率密度函数所采集,确定粒子为UKF中采集Sigma点后所求取的系统状态值.由此降低了PF处理粒子时的复杂程度以及粒子退化的程度.仿真结果表明:相比于UKF算法,该方法有效提高了组合导航系统的精度,具有较好的鲁棒性.

GAO Honglian, YOU Jie, CAO Songyin.

In-flight alignment method of integrated SINS/GPS navigation system based on combined PF-UKF filter

[J]. Journal of Shanghai Jiao Tong University, 2022, 56(11): 1447-1452.

[本文引用: 1]

ZHU J, LI A, QIN F, et al.

A new robust adaptive filter aided by machine learning method for SINS/DVL integrated navigation system

[J]. Sensors, 2022, 22(10): 3792-3813.

DOI:10.3390/s22103792      URL     [本文引用: 1]

As an important means of underwater navigation and positioning, the accuracy of SINS/DVL integrated navigation system greatly affects the efficiency of underwater work. Considering the complexity and change of the underwater environment, it is necessary to enhance the robustness and adaptability of the SINS/DVL integrated navigation system. Therefore, this paper proposes a new adaptive filter based on support vector regression. The method abandons the elimination of outliers generated by Doppler Velocity Logger (DVL) in the measurement process from the inside of the filter in the form of probability density function modeling. Instead, outliers are eliminated from the perspective of external sensors, which effectively improves the robustness of the filter. At the same time, a new Variational Bayesian (VB) strategy is adopted to reduce the influence of inaccurate process noise and measurement noise, and improve the adaptiveness of the filter. Their advantages complement each other, effectively improve the stability of filter. Simulation and ship-borne tests are carried out. The test results show that the method proposed in this paper has higher navigation accuracy.

YAR Y, XU X, YANG D, et al.

An IMM-UKF aided SINS/USBL calibration solution for underwater vehicles

[J]. IEEE Transactions on Vehicular Technology, 2020, 69(4): 3740-3747.

DOI:10.1109/TVT.25      URL     [本文引用: 1]

NORDIN M H, SHARMA S, KHAN A, et al.

Collaborative unmanned vehicles for inspection, maintenance, and repairs of offshore wind turbines

[J]. Drones, 2022, 6(6): 137-163.

DOI:10.3390/drones6060137      URL     [本文引用: 1]

Operations and maintenance of Offshore Wind Turbines (OWTs) are challenging, with manual operators constantly exposed to hazardous environments. Due to the high task complexity associated with the OWT, the transition to unmanned solutions remains stagnant. Efforts toward unmanned operations have been observed using Unmanned Aerial Vehicles (UAVs) and Unmanned Underwater Vehicles (UUVs) but are limited mostly to visual inspections only. Collaboration strategies between unmanned vehicles have introduced several opportunities that would enable unmanned operations for the OWT maintenance and repair activities. There have been many papers and reviews on collaborative UVs. However, most of the past papers reviewed collaborative UVs for surveillance purposes, search and rescue missions, and agricultural activities. This review aims to present the current capabilities of Unmanned Vehicles (UVs) used in OWT for Inspection, Maintenance, and Repair (IMR) operations. Strategies to implement collaborative UVs for complex tasks and their associated challenges are discussed together with the strategies to solve localization and navigation issues, prolong operation time, and establish effective communication within the OWT IMR operations. This paper also briefly discusses the potential failure modes for collaborative approaches and possible redundancy strategies to manage them. The collaborative strategies discussed herein will be of use to researchers and technology providers in identifying significant gaps that have hindered the implementation of full unmanned systems which have significant impacts towards the net zero strategy.

GAN W, ZHU D, JI D.

QPSO-model predictive control-based approach to dynamic trajectory tracking control for unmanned underwater vehicles

[J]. Ocean Engineering, 2018, 158: 208-220.

DOI:10.1016/j.oceaneng.2018.03.078      URL     [本文引用: 1]

张文佳, 马辛.

深空探测器接近段自主导航的滑动窗口自适应滤波方法

[J]. 上海交通大学学报, 2022, 56(11): 1461-1469.

DOI:10.16183/j.cnki.jsjtu.2022.233      [本文引用: 1]

当深空探测器接近目标行星时,由于目标行星引力快速增加,轨道动力学模型会出现较快的加速度变化.由于噪声协方差不完全已知,所以传统的滤波方法无法获得导航参数的最优估计,难以满足接近段导航系统的性能要求.为满足系统高稳定性和高精度需求,提出一种基于系统噪声协方差的滑动窗口自适应非线性滤波方法.通过构造系统噪声协方差更新函数,使用滑动窗口对噪声协方差平稳化处理,将速度噪声引起的误差与位置噪声引起的误差隔离开,实时更新所使用的滤波参数信息,自适应调节系统噪声协方差.以火星探测器为例进行仿真,仿真结果表明,相对于传统的无迹卡尔曼滤波方法,该方法获取的位置精度和速度精度分别提高90.97%和66.17%,抑制了系统模型上快速变化的积分误差,并解决传统滤波方法的发散问题.此外,分析了滤波周期和窗口大小对导航性能的影响,为深空探测自主导航提供了一种可行的自适应滤波新方法.

ZHANG Wenjia, MA Xin.

A sliding window adaptive filtering algorithm for autonomous navigation of the approach phase of deep space probe

[J]. Journal of Shanghai Jiao Tong University, 2022, 56(11): 1461-1469.

[本文引用: 1]

姜俊豪, 陈刚.

驾驶机器人转向操纵的动态模型预测控制方法

[J]. 上海交通大学学报, 2022, 56(5): 594-603.

DOI:10.16183/j.cnki.jsjtu.2021.108      [本文引用: 1]

为实现驾驶机器人对试验车辆进行精确的转向操纵,提出了一种驾驶机器人转向操纵的动态模型预测控制方法.建立了驾驶机器人与被操纵车辆的耦合动力学模型,并对耦合模型的可控性进行判别.利用卡尔曼滤波器对耦合模型进行状态估计,并结合估计状态设计了模型预测控制器.将最小二乘法用于路径曲率与预测时域的非线性关系的拟合,设计了具有可变预测时域的动态模型预测控制器.进行了不同驾驶工况下的驾驶机器人转向操纵仿真与试验,研究结果表明了所提方法的有效性.

JIANG Junhao, CHEN Gang.

Dynamic model predictive control method for steering control of driving robot

[J]. Journal of Shanghai Jiao Tong University, 2022, 56(5): 594-603.

[本文引用: 1]

ZHAO W, ZHAO H, LIU G, et al.

ANFIS-EKF-based single-beacon localization algorithm for AUV

[J]. Remote Sensing, 2022, 14(20): 5281-5301.

DOI:10.3390/rs14205281      URL     [本文引用: 1]

Singe-beacon localization technology can help Autonomous Underwater Vehicles (AUVs) to obtain precise positions by deploying only one beacon. It is considered as a promising way, benefiting from saving much time and labor compared with traditional Long-Baseline Localization (LBL). A typical single-beacon localization scheme contains two essential questions: the initial observability problem and long-endurance trajectory tracking problem. Aiming at these core problems, a comprehensive solution for single-beacon localization is described in this paper. An multi-hypothesis initial position discriminant method is proposed firstly, which helps to achieve accurate initial location based on observability analysis. Then, an Adaptive Network Fuzzy Inference System (ANFIS)-improved Extended Kalman Filter (EKF) method is proposed, in which single-beacon measuring information is fused with off-the-shelf sensors, including DVL, Compass, etc. ANFIS-EKF can help to improve trajectory tracking precisions by restraining the heavy loss of linearization in conventional EKF. Both simulation and field tests are conducted to verify the performance of the proposed algorithms.

AL B N, GAVRILOV A.

A novel approach for aiding unscented Kalman filter for bridging GNSS outages in integrated navigation systems

[J]. Navigation, 2021, 68(3): 521-539.

DOI:10.1002/navi.v68.3      URL     [本文引用: 1]

YANG J, GAO S, LI G, et al.

An adaptive cubature Kalman filtering algorithm based on variational mode decomposition for pulsar navigation

[J]. IET Communications, 2022, 16(16): 1982-1992.

DOI:10.1049/cmu2.v16.16      URL     [本文引用: 1]

秦康, 董新民, 陈勇, 等.

基于正交变换的改进CKF算法

[J]. 控制与决策, 2018, 33(2): 330-336.

[本文引用: 3]

QIN Kang, DONG Xinmin, CHEN Yong, et al.

Modified CKF algorithm based on orthogonal transformation

[J]. Control and Decision, 2018, 33(2): 330-336.

[本文引用: 3]

赵丽, 薛建平.

基于采样点正交变换的改进CKF算法

[J]. 计算机工程与应用, 2018, 54(18): 45-51.

DOI:10.3778/j.issn.1002-8331.1707-0344      [本文引用: 3]

针对传统CKF算法在解决高维问题时因非局部采样造成的滤波性能下降问题，基于设计的正交矩阵提出了一种改进的CKF算法。采用多元Taylor级数展开，揭示了CKF虽能解决UKF的数值不稳定性问题，但同时也引入了非局部采样问题这一事实；进一步设计出一种正交变换矩阵，用于对CKF算法中的采样点进行变换，并从理论上证明了提出的改进CKF算法相对于CKF在高维、强非线性等非局部采样问题突出的应用场合具有更高的估计精度。仿真结果验证了算法的有效性。

ZHAO Li, XUE Jianping.

Improved CKF based on orthogonal transformation

[J]. Computer Engineering and Applications, 2018, 54(18): 45-51.

DOI:10.3778/j.issn.1002-8331.1707-0344      [本文引用: 3]

In order to solve the nonlocal sampling problem inherent in CKF for high dimensional problems, a new methodology based on orthogonal matrix is proposed in this paper. The performance of the UT is analyzed based on the multi-dimensional Taylor series, to illustrate that even the problem of numerical instability of UKF can be solved by CKF, but the nonlocal sampling problem is introduced. Through the orthogonal matrix the new algorithm named ICKF is proposed in this paper. It is proved theoretically that ICKF has higher estimation accuracy than CKF in the high-dimentional and strongly nonlinearity situation, where local sampling problems is prominent. Simulation example verifies the effectiveness of the proposed algorithm.

严恭敏, 严卫生, 徐德民.

基于欧拉平台误差角的SINS非线性误差模型研究

[J]. 西北工业大学学报, 2009, 27(4): 511-516.

[本文引用: 1]

YAN Gongmin, YAN Weisheng, XU Demin.

A SINS nonlinear error model reflecting better characteristics of SINS errors

[J]. Journal of Northwestern Polytechnical University, 2009, 27(4): 511-516.

[本文引用: 1]

王茂松, 吴文启, 何晓峰, 等.

状态变换卡尔曼滤波的进一步解释及应用

[J]. 中国惯性技术学报, 2019, 27(4): 499-504.

[本文引用: 1]

WANG Maosong, WU Wenqi, HE Xiaofeng, et al.

Further explanation and application of state transformation extended Kalman filter

[J]. Journal of Chinese Inertial Technology, 2019, 27(4): 499-504.

[本文引用: 1]

ISERLES A, MUNTHE K H Z, NORSETT S P, et al.

Lie-group methods

[J]. Acta Numerica, 2000, 9: 215-365.

DOI:10.1017/S0962492900002154      URL     [本文引用: 1]

Many differential equations of practical interest evolve on Lie groups or on \nmanifolds acted upon by Lie groups. The retention of Lie-group structure \nunder discretization is often vital in the recovery of qualitatively correct geometry \nand dynamics and in the minimization of numerical error. Having \nintroduced requisite elements of differential geometry, this paper surveys the \nnovel theory of numerical integrators that respect Lie-group structure, highlighting \ntheory, algorithmic issues and a number of applications.

CUI J, WANG M, WU W, et al.

Lie group based nonlinear state errors for MEMS-IMU/GNSS/magnetometer integrated navigation

[J]. Journal of Navigation, 2021, 74(4): 887-900.

DOI:10.1017/S037346332100014X      URL     [本文引用: 2]

In the integrated navigation system using extended Kalman filter (EKF), the state error conventionally uses linear approximation to tackle the commonly nonlinear problem. However, this error definition can diverge the filter in some adverse situations due to significant distortion of the linear approximation. By contrast, the nonlinear state error defined in the Lie group satisfies the autonomous equation, which thus has distinctively better convergence property. This work proposes a novel strapdown inertial navigation system (SINS) nonlinear state error defined in the Lie group and derives the SINS equations of the Lie group EKF (LG-EKF) for the MIMU/GNSS/magnetometer integrated navigation system. The corresponding measurement equations are also derived. A land vehicle field test has been conducted to evaluate the performance of EKF, ST-EKF (state transformation extended Kalman filter) and LG-EKF, which verifies LG-EKF's superior estimation accuracy of the heading angle as well as the other two horizontal angles (pitch and roll). The LG-EKF proposed in this paper is unlimited in the choice of sensors, which means it can be applied with both high-end and low-end inertial sensors.

CHANG L, DI J, QIN F.

Inertial based integration with transformed INS mechanization in earth frame

[J]. IEEE/ASME Transactions on Mechatronics. 2021, 27: 1738-1749.

DOI:10.1109/TMECH.2021.3090428      URL     [本文引用: 2]

QIAN L, QIN F, LI K, et al.

Research on the necessity of lie group strapdown inertial integrated navigation error model based on Euler angle

[J]. Sensors, 2022, 22(20): 7742-7761.

DOI:10.3390/s22207742      URL     [本文引用: 1]

In response to the lack of specific demonstration and analysis of the research on the necessity of the Lie group strapdown inertial integrated navigation error model based on the Euler angle, two common integrated navigation systems, strapdown inertial navigation system/global navigation satellite system (SINS/GNSS) and strapdown inertial navigation system/doppler velocity log (SINS/DVL), are used as subjects, and the piecewise constant system (PWCS) matrix, based on the Lie group error model, is established. From three aspects of variance estimation, the observability and performance of the system with large misalignment angles for low, medium, and high accuracy levels, traditional error model, Lie group left error model, and right error model are compared. The necessity of research on Lie group error model is analyzed quantitatively and qualitatively. The experimental results show that Lie group error model has better stability of variance estimation, estimation accuracy, and observability than traditional error model, as well as higher practical value.

AXEL B, SILVERE B.

Invariant Kalman filtering

[J]. Annual Review of Control, Robotics, and Autonomous Systems, 2018, 1(1): 237-257.

DOI:10.1146/control.2018.1.issue-1      URL     [本文引用: 1]

LUO Y, GUO C, LIU J.

Equivariant filtering framework for inertial-integrated navigation

[J]. Satellite Navigation. 2021, 2: 1-17.

DOI:10.1186/s43020-020-00033-9      [本文引用: 3]

Because of its high-precision, low-cost and easy-operation, Precise Point Positioning (PPP) becomes a potential and attractive positioning technique that can be applied to self-driving cars and drones. However, the reliability and availability of PPP will be significantly degraded in the extremely difficult conditions where Global Navigation Satellite System (GNSS) signals are blocked frequently. Inertial Navigation System (INS) has been integrated with GNSS to ameliorate such situations in the last decades. Recently, the Visual-Inertial Navigation Systems (VINS) with favorable complementary characteristics is demonstrated to realize a more stable and accurate local position estimation than the INS-only. Nevertheless, the system still must rely on the global positions to eliminate the accumulated errors. In this contribution, we present a semi-tight coupling framework of multi-GNSS PPP and Stereo VINS (S-VINS), which achieves the bidirectional location transfer and sharing in two separate navigation systems. In our approach, the local positions, produced by S-VINS are integrated with multi-GNSS PPP through a graph-optimization based method. Furthermore, the accurate forecast positions with S-VINS are fed back to assist PPP in GNSS-challenged environments. The statistical analysis of a GNSS outage simulation test shows that the S-VINS mode can effectively suppress the degradation of positioning accuracy compared with the INS-only mode. We also carried out a vehicle-borne experiment collecting multi-sensor data in a GNSS-challenged environment. For the complex driving environment, the PPP positioning capability is significantly improved with the aiding of S-VINS. The 3D positioning accuracy is improved by 49.0% for Global Positioning System (GPS), 40.3% for GPS + GLOANSS (Global Navigation Satellite System), 45.6% for GPS + BDS (BeiDou navigation satellite System), and 51.2% for GPS + GLONASS + BDS. On this basis, the solution with the semi-tight coupling scheme of multi-GNSS PPP/S-VINS achieves the improvements of 41.8–60.6% in 3D positioning accuracy compared with the multi-GNSS PPP/INS solutions.

WANG M, WU W, ZHOU P, et al.

State transformation extended Kalman filter GPS/SINS tightly coupled integration

[J]. GPS Solution. 2018, 22: 1-12.

DOI:10.1007/s10291-017-0674-x      URL     [本文引用: 1]

DU S, HUANG Y, LIN B, et al.

A lie group manifold-based nonlinear estimation algorithm and its application to low-accuracy SINS/GNSS integrated navigation

[J]. IEEE Transactions on Instrumentation and Measurement. 2022, 71: 1-27.

[本文引用: 1]

ZHU T, LI A, LI K, et al.

The quaternion based error model based on SE(3) of the INS

[J]. IEEE Sensors Journal, 2022, 22(13): 13067-13077.

DOI:10.1109/JSEN.2022.3174596      URL     [本文引用: 1]

FAN X, WANG G, HAN J, et al.

Interacting multiple model based on maximum correntropy Kalman filter

[J]. IEEE Transactions on Circuits and Systems II—Express Briefs, 2021, 68(8): 3017-3021.

[本文引用: 1]

ZHU B, CHANG L, XU J, et al.

Huber-based adaptive unscented Kalman filter with non-Gaussian measurement noise

[J]. Circuits, Systems, and Signal Processing, 2018, 37(9): 3842-3861.

DOI:10.1007/s00034-017-0736-x      [本文引用: 1]

张文杰, 王世元, 冯亚丽, 等.

基于Huber的高阶容积卡尔曼跟踪算法

[J]. 物理学报, 2016, 65(8): 358-366.

[本文引用: 1]

ZHANG Wenjie, WANG Shiyuan, FENG Yali, et al.

Huber-based high-degree cubature Kalman tracking algorithm

[J]. Acta Physica Sinica, 2016, 65(8): 358-366.

[本文引用: 1]

黄玉, 武立华, 孙枫.

基于Huber M估计的鲁棒Cubature卡尔曼滤波算法

[J]. 控制与决策, 2014, 29(3): 572-576.

[本文引用: 1]

HUANG Yu, WU Lihua, SUN Feng.

Robust Cubature Kalman filter based on Huber M estimator

[J]. Control and Decision, 2014, 29(3): 572-576.

[本文引用: 1]

CANTELOBRE T, CHAHBAZIAN C, CROUX A, et al.

A real-time unscented Kalman filter on manifolds for challenging AUV navigation

[C]// 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems. Providence, Las Vegas, NV, USA: IEEE, 2020: 2309-2316.

[本文引用: 1]

LUO L, ZHANG Y, FANG T, et al.

A new robust Kalman filter for SINS/DVL integrated navigation system.

[J]. IEEE Access, 2019, 7: 51386-51395.

DOI:10.1109/ACCESS.2019.2911110      [本文引用: 1]

A new robust strap-down inertial navigation system (SINS) and Doppler velocity log (DVL) integrated navigation algorithm are proposed in this paper with a focus on suppressing the process uncertainty and measurement outliers induced by severe manoeuvering. In the proposed algorithm, the one-step predicted probability density function is modeled as Student's t-distribution to deal with the heavy-tailed process noise, and hierarchical Gaussian state-space model for SINS/DVL integrated navigation algorithm is constructed. To detect and eliminate the measurement outliers, each measurement is marked by a binary indicator variable modeled as a beta-Bernoulli distribution. The variational Bayesian approach is used to jointly estimate state vector, auxiliary random variable, scale matrix, Bernoulli variable, and beta variable. The experimental results illustrate that the proposed algorithm has better robustness and navigation accuracy to deal with process uncertainty and measurement outliers than existing state-of-the-art algorithms.

朱天高, 刘勇, 李开龙, 等.

基于欧拉角的李群捷联惯导误差模型分析与比较研究

[J]. 系统工程与电子技术, 2023, 45(10): 3265-3273.

DOI:10.12305/j.issn.1001-506X.2023.10.31      [本文引用: 2]

惯导姿态、速度状态量同时纳入一个群中, 且按照误差的两种定义可构成李群左误差模型和右误差模型。针对基于欧拉角的两种李群误差模型进行了比较研究, 分析了两种李群误差模型的差异。针对捷联惯性导航系统(strapdown inertial navigation system, SINS)/全球定位系统(global position system, GPS)和SINS/多普勒计程仪(Doppler velocity log, DVL)两种典型组合导航系统应用, 提出合适的误差模型选择方案。车载和船载实验结果表明, 两种李群误差模型分别更适用于SINS/GPS和SINS/DVL组合导航系统, 尤其是在大失准角条件下效果更为显著, 两种李群误差模型具有更好的定位精度、收敛速度和稳定性。

ZHU Tiangao, LIU Yong, LI Kailong, et al.

Analysis and comparison of Euler angles based error model based on Lie groups of the SINS

[J]. Systems Engineering and Electronics, 2023, 45(10): 3265-3273.

DOI:10.12305/j.issn.1001-506X.2023.10.31      [本文引用: 2]

The attitude and velocity of inertial navigation are included in a group at the same time. Accor-ding to two kinds of definition of error, the left error model and right error model of Lie group are formed. In view of two Lie group error models based on Euler angles, a comparison and analysis of two error models are carried out, and the difference of two error models based on Lie group is given. In view of two typical integrated navigation system of strapdown inertial navigation system (SINS)/global position system (GPS) and SINS/Doppler velocity log (DVL), a suitable error model selection scheme is proposed. The vehicle test and boat test results show that the two error models based on Lie Group are more suitable to SINS/GPS and SINS/DVL respectively, especially with large initial misalignment angles. The selected error model has a better position accuracy, convergence speed and stability.

HUBER P J.

Robust estimation of a location parameter

[J]. The Annals of Mathematical Statistics, 1964, 35(1): 73-101.

DOI:10.1214/aoms/1177703732      URL     [本文引用: 1]

HOU J, HE H, YANG Y, et al.

A variational Bayesian and Huber-based robust square root cubature Kalman filter for lithium-ion battery state of charge estimation

[J]. Energies, 2019, 12(9): 1717-1739.

DOI:10.3390/en12091717      URL     [本文引用: 1]

An accurate state of charge (SOC) estimation is vital for safe operation and efficient management of lithium-ion batteries. To improve the accuracy and robustness, an adaptive and robust square root cubature Kalman filter based on variational Bayesian approximation and Huber’s M-estimation (VB-HASRCKF) is proposed. The variational Bayesian (VB) approximation is used to improve the adaptivity by simultaneously estimating the measurement noise covariance and the SOC, while Huber’s M-estimation is employed to enhance the robustness with respect to the outliers in current and voltage measurements caused by adverse operating conditions. A constant-current discharge test and an urban dynamometer driving schedule (UDDS) test are performed to verify the effectiveness and superiority of the proposed algorithm by comparison with the square root cubature Kalman filter (SRCKF), the VB-based SRCKF, and the Huber-based SRCKF. The experimental results show that the proposed VB-HASRCKF algorithm outperforms the other three filters in terms of SOC estimation accuracy and robustness, with a little higher computation complexity.

/

〈 〉