申请/专利权人：哈尔滨工业大学

申请日：2017-11-06

公开（公告）日：2020-09-11

公开（公告）号：CN107990910B

主分类号：G01C25/00(20060101)

分类号：G01C25/00(20060101)

优先权：

专利状态码：有效-授权

法律状态：2020.09.11#授权;2018.06.01#实质审查的生效;2018.05.04#公开

摘要：本发明公开了一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法。首先，将子惯导加速度计的比力输出转换到导航坐标系，利用巴特沃斯数字低通滤波器对其进行滤波处理；其次，主、子惯导分别进行惯导解算，主惯导的速度、姿态和角速度信息传输到子惯导的导航计算机，利用主、子惯导系统之间的速度误差、姿态误差和角速度误差构造量测量；然后，采用速度加姿态加角速度的匹配方式，建立大方位失准角情况下的状态方程和量测方程；最后，利用所建立的状态方程和量测方程，进行容积卡尔曼滤波解算，估计子惯导系统与主惯导系统间的安装误差角，完成传递对准。本发明解决了舰船在大方位失准角与大杆臂误差情况下的快速高精度对准问题。

主权项：1.一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法，其特征在于，包括以下步骤：步骤一：完成子惯导系统的启动、预热准备；步骤二：将子惯导加速度计的比力输出转换到导航坐标系，利用巴特沃斯数字低通滤波器对其进行滤波处理，以达到消除杆臂效应误差影响的目的；步骤三：主、子惯导系统分别进行惯导解算，主惯导的速度、姿态和角速度信息传输到子惯导的导航计算机；步骤四：在舰船存在大方位失准角的情况下，采用速度加姿态加角速度的匹配方式，认为主、子惯导载体坐标系不同而导航坐标系相同，选取主、子惯导系统之间的速度误差、姿态误差和角速度误差作为观测量，建立系统的状态方程和量测方程；设主、子惯导的导航坐标系为n系，主、子惯导的载体坐标系分别用m和s表示，则待估计量为m系和s系之间的安装误差角由于在对准之前，子惯导无法获取子惯导载体坐标系s相对于导航坐标系n的捷联矩阵，因此先定义子惯导计算载体坐标系将与m之间的误差角称为量测失准角，表示为根据比力方程，对主、子惯导有： 式中，和分别为主、子惯导测量的比力在各自载体坐标系的投影，和为主、子惯导的速度在导航坐标系的投影；为主惯导载体坐标系到导航坐标系的方向余弦矩阵；为子惯导载体坐标系到导航坐标系的方向余弦矩阵；为地球自转角速度在导航坐标系的投影；为n系相对于地球坐标系的角速度在n系的投影；和分别为主、子惯导所在位置的重力加速度；两式相减得： 不考虑杆臂效应误差，主、子惯导输出比力关系如下： 式中，为子惯导加速度计误差；为主惯导载体坐标系到子惯导载体坐标系的方向余弦矩阵；认为得到： 上式即为速度加姿态加角速度匹配传递对准的速度误差微分方程；量测失准角的微分为s′系相对于m系的角速度在s′系的投影，即： 不考虑挠曲变形，主、子惯导输出角速度关系如下： 因此有： 由于主、子惯导固连在运载体上，故认为安装误差角为常值，因此： 上述两式即为速度加姿态加角速度匹配传递对准的量测失准角微分方程和安装误差角微分方程；惯性器件的主要误差源有陀螺漂移ε和加速度计零偏陀螺漂移主要由常值漂移εc、相关漂移εr、随机白噪声漂移wg三部分组成；相关漂移的相关时间大于1小时，可近似视为常值漂移，且比常值漂移小1-2个数量级，故陀螺误差模型简化为： 类似的，加速度计零偏也可简化为常值漂移，即： 忽略垂向通道，选取的状态变量为： 系统状态方程为： 其中，n为导航坐标系；m系为主惯导载体坐标系；s系为子惯导载体坐标系；为子惯导计算载体坐标系；δVn为速度误差在导航坐标系的投影；为主惯导载体坐标系到子惯导载体坐标系的方向余弦矩阵；为主惯导载体坐标系到子惯导计算载体坐标系的方向余弦矩阵；为主惯导载体坐标系到导航坐标系的方向余弦矩阵；为子惯导测量的比力在其载体坐标系的投影；为地球自转角速度在导航坐标系的投影；为n系相对于地球坐标系的角速度在n系的投影；为s系和m系之间的安装误差角；为系和m系之间的量测失准角；为主惯导相对于导航坐标系的角速度在m系的投影；为加速度计常值漂移；wv为加速度计随机漂移；εs为陀螺常值漂移；为陀螺随机漂移；选取主、子惯导间的速度误差、量测失准角和角速度误差作为观测量，即： 其中，速度观测量为δVn，姿态观测量为角速度观测量为量测方程为：Z＝hX+V其中，V为系统的量测噪声；步骤五：利用所建立的状态方程和量测方程，进行容积卡尔曼滤波解算，估计子惯导系统与主惯导系统间的安装误差角，完成传递对准。

全文数据：一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法技术领域[0001]本发明涉及捷联惯导技术领域，特别是涉及一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法。背景技术[0002]惯性导航系统是一种基于惯性原理的自主式导航系统。捷联惯导系统将陀螺和加速度计直接固连在运载体上来测量运载体的角运动和线运动信息，经过积分运算推算出运载体相对于地球的速度、位置以及姿态和航向信息。初始对准是捷联惯导系统的一项关键技术，对准的精度直接影响到导航系统的精度，而完成对准的时间则直接影响到系统的快速反应能力。传递对准是利用主惯导的输出信息来对准子惯导的一种初始对准方式，对准速度快，且对载体的机动不加限制。[0003]由于舰船在海上航行会受到海浪影响，尤其是海况不佳的情况下，基于大方位失准角的非线性模型更能准确地描述真实的系统。对于大型舰船而言，舰船上的主惯导往往安装在其摇摆中心，而舰载设备上的子惯导的安装位置与主惯导有很长一段距离，当载体存在角运动时，主、子惯导加速度计会敏感到不同的加速度，从而导致主、子惯导之间存在杆臂速度差，这就是传递对准中的杆臂效应现象。杆臂效应会严重影响传递对准的精度和收敛速度，必须进行补偿。[0004]高伟等人在《传递对准中杆臂效应的误差分析与补偿》发表于期刊《仪器仪表学报》，2013年，第34卷，03期一文中，针对大方位失准角条件下的非线性系统提出了一种直接计算的补偿方法，方位失准角可在120s内收敛至0.381°。黄湘远等人在《基于简化CKF降维CKF混合滤波的非线性对准技术研究》发表于期刊《弹箭与制导学报》，2015年，第35卷，01期一文中，提出了基于简化CKF降维CKF混合滤波的非线性对准方法，大大减小了计算量，水平对准精度达到1以下，方位对准精度达到5以下。徐晓苏等人在《基于改进型CKF的SINS初始对准方法》华中科技大学学报（自然科学版），2016年，第44卷，01期一文中，提出了一种改进型CKF方法用于大方位失准角条件下的传递对准，方位对准精度达到3以下。本发明设计了一种基于容积卡尔曼滤波的速度加姿态加角速度传递对准方法，可用于舰船存在大方位失准角与大杆臂误差的情况，对准的速度和精度比现有方法均有很大程度的提尚。发明内容[0005]本发明的目的在于提供一种可应用于舰船存在大方位失准角以及大杆臂误差情况下的快速高精度传递对准方法。[0006]实现本发明目的的技术方案为:一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法，包括以下步骤：[0007]步骤一:完成子惯导系统的启动、预热准备；[0008]步骤二:将子惯导加速度计的比力输出转换到导航坐标系，利用巴特沃斯数字低通滤波器对其进行滤波处理，以达到消除杆臂效应误差影响的目的；[0009]步骤三:主、子惯导系统分别进行惯导解算，主惯导的速度、姿态和角速度信息传输到子惯导的导航计算机；[0010]步骤四：在舰船存在大方位失准角的情况下，采用速度加姿态加角速度的匹配方式，认为主、子惯导载体坐标系不同而导航坐标系相同，选取主、子惯导系统之间的速度误差、姿态误差和角速度误差作为量测量，建立系统的状态方程和量测方程；[0011]步骤五:利用所建立的状态方程和量测方程，进行容积卡尔曼滤波解算，估计子惯导系统与主惯导系统间的安装误差角，完成传递对准。[0012]在步骤二中，巴特沃斯数字低通滤波器的技术要求为：[0013]通带截止频率为fP=0.0IHz，通带波纹为ap=2dB，阻带截止频率为fs=0.15Hz，阻带衰减为as=40dB。[0014]设计出二阶巴特沃斯数字滤波器的离散传递函数为：[0015][0016]则滤波器的状态方程为：[0017][0018]输出方程为：[0019][0020]其中，uη表示滤波器的输入，，_c=[0.001660.70710]，d=5.53551e-06。[0021]在步骤三中，建立的速度加姿态加角速度匹配传递对准数学模型如下：[0022]忽略垂向通道，选取的状态变量为：[0023][0024]系统状态方程为：[0025][0026]其中，η为导航坐标系;m系为主惯导载体坐标系；s系为子惯导载体坐标系；f为子惯导计算载体坐标系；SVn为速度误差在导航坐标系的投影；（为主惯导载体坐标系到子惯导载体坐标系的方向余弦矩阵为主惯导载体坐标系到子惯导计算载体坐标系的方向余弦矩阵；为主惯导载体坐标系到导航坐标系的方向余弦矩阵；为子惯导测量的比力在其载体坐标系的投影；为地球自转角速度在导航坐标系的投影；，为11系相对于地球坐标系的角速度在η系的投影；为S系和m系之间的安装误差角；，为i系和m系之间的量测失准角I为主惯导相对于导航坐标系的角速度在m系的投影；为加速度计常值漂移;Wv为加速度计随机漂移;es为陀螺常值漂移，.为陀螺随机漂移。[0027]采用速度加姿态加角速度的匹配方式，将主、子惯导间的速度误差δνη、量测失准角》以及角速度误差乍为观测量：[0028][0029]量测方程为：[0030]z=h⑻+V[0031]其中，V为系统的量测噪声。[0032]利用三自由度Spherical-Radial求容积规则，采用一组2n个等权值的容积点设计非线性滤波算法即容积卡尔曼滤波，具体如下：[0033]对于如下一个连续非线性系统：[0034][0035]采用4阶龙格库塔RungeKutta法将系统模型进行离散化，得到一个离散非线性系统：[0036][0037]其中，Xk为系统状态矢量;Zk为观测矢量;Wk为系统噪声矢量，Vk为量测噪声矢量，二者均为零均值的高斯白噪声序列，且互不相关，即满足：[0038][0039]其中，Qk为系统噪声序列的方差阵;Rk为量测噪声序列的方差阵;Skj为克罗内克函数。[0040]容积卡尔曼滤波的具体实现步骤如下：[0041]a.时间更新[0042]假设k-Ι时刻的状态xk-i的统计特性已知，先对Pk-ι做Cholesky分解：[0043][0044]计算容积点：[0045][0046]计算经系统状态方程传递后的容积点：[0047][0048]估计k时刻的状态预测值：[0049][0050]估计k时刻的状态预测协方差阵：[0051][0052]b.量测更新[0053]对Pkk-i做Cholesky分解：[0054][0055]计算容积点：[0056][0057]计算经系统量测方程传递后的容积点：[0058]Zi,kk-i=hXi,kk-ii=1，2…，2n[0059]估计k时刻的量测预测值：[0060][0061]估计k时刻的量测预测协方差阵：[0062][0063]估计k时刻的一步预测互相关协方差阵：[0064][0065]估计k时刻的滤波增益：[0066][0067]求取k时刻的状态估计值：[0068][0069]求取k时刻的状态误差协方差阵：[0070][0071]根据建立的速度加姿态加角速度匹配传递对准的状态方程和量测方程，进行容积卡尔曼滤波解算，估计出子惯导系统与主惯导系统间的安装误差角，完成传递对准。[0072]与现有技术相比，本发明的有益效果是：[0073]本发明在舰船存在大方位失准角的情况下，将系统考虑为非线性模型，并且设计巴特沃斯数字低通滤波器对子惯导加速度计的输出进行滤波处理，采用速度加姿态加角速度的匹配方式，建立滤波模型，进行容积卡尔曼滤波解算，有效地消除了杆臂效应的影响，大幅度地提高了舰船在大方位失准角与大杆臂误差情况下的对准速度和精度。附图说明[0074]图1为本发明的基本流程框图；[0075]图2为杆臂加速度的频谱；[0076]图3为利用Matlab仿真得到的安装误差角估计误差曲线。具体实施方式[0077]下面结合附图对本发明进一步说明。[0078]为了验证本发明的有效性，利用Matlab对设计的基于容积卡尔曼滤波的舰船大方位失准角时的传递对准非线性模型进行仿真。[0079]舰船在海中航行时会受到风浪影响，产生三轴摇摆运动，其数学模型为：[0080][0081]式中，Φ，θ，γ分别表示航向角、纵摇角和横摇角;I]m，0m，摇摆角幅值；ωγ，ωρ，为摇摆角频率;Ti=SVoi,i=y，p，r为摇摆周期；为初始姿态角;K为初始航向。[0082]仿真参数设置如下：[0083]摇摆角幅值:i]m=5°，0m=15°，ym=10°;[0084]摇摆周期：Ty=8s，TP=12s，Tr=6s;[0085]初始姿态角：[0086]初始航向:K=30°;[0087]初始炜度4刀始经度λ=126.6705°;[0088]误差角为：[0089]陀螺常值漂移为εχ=εΥ=εζ=〇.〇I°h，随机漂移为0.001°h;[0090]加速度计随机常值偏置为10_4g，加速度计随机漂移为10_5g;[0091]舰船以5ms的速度匀速直航；[0092]滤波周期：0.05s;[0093]杆臂长度:rs=[8252]Tm。[0094]设计巴特沃斯数字低通滤波器对子惯导加速度计输出进行处理，具体步骤如下：[0095]子惯导系统的振荡是以舒拉周期和地球自转周期为主的振荡，处在低频区，其频谱分布在f=2XHT4Hz以下。根据杆臂效应加速度的频谱，可确定巴特沃斯低通滤波器的技术要求：[0096]通带截止频率fP=0.0IHz，通带波纹ap=2dB，阻带截止频率fs=0.15Hz，阻带衰减as=40dB。首先通过下式确定滤波器的阶数N。[0097][0098]式中，代入可得N=I.80,取N=2。[0099]3dB截止频率为：〇[0100]由巴特沃斯归一化低通滤波器参数表可得二阶低通滤波器的归一化原型系统函数：[0101][0102]将63p去归一化，得到模拟低通滤波器的系统函数：[0103][0104]采样间隔T=0.05s，利用双线性变换法转换，得到数字低通滤波器的系统函数：[0105][0106]滤波器的状态方程为：[0107][0108]输出方程为：[0109][0110]其中，uη表示滤波器的输入，c=[0.001660.70710]，d=5.53551e-06。[0111]建立速度加姿态加角速度匹配传递对准数学模型，具体步骤如下：[0112]速度加姿态加角速度匹配传递对准数学建模的出发点是认为主、子惯导的载体坐标系不同，而导航坐标系相同。设主、子惯导的导航坐标系为η系，主、子惯导的载体坐标系分别用m和s表示，则待估计量为m系和s系之间的安装误差角。由于在对准之前，子惯导无法获取子惯导载体坐标系s相对于导航坐标系η的捷联矩阵，因此先定义子惯导计算载体坐标系1，将i与m之间的误差角称为量测失准角，表示为[0113]根据比力方程，对主、子惯导有：[0114][0115]式中分别为主、子惯导测量的比力在各自载体坐标系的投影，为主、子惯导的速度在导航坐标系的投影，为主惯导载体坐标系到导航坐标系的方向余弦矩阵为子惯导载体坐标系到导航坐标系的方向余弦矩阵；为地球自转角速度在导航坐标系的投影为η系相对于地球坐标系的角速度在η系的投影；分别为主、子惯导所在位置的重力加速度。[0116]两式相减得：[0117][0118]不考虑杆臂效应误差，主、子惯导输出比力关系如下：[0119][0120]式中，Vi为子惯导加速度计误差：为主惯导载体坐标系到子惯导载体坐标系的方向余弦矩阵。认为，可得：[0121][0122]上式即为速度加姿态加角速度匹配传递对准的速度误差微分方程。[0123]量测失准角的微分为s'系相对于m系的角速度在S系的投影，SP:[0124][0125]不考虑挠曲变形，主、子惯导输出角速度关系如下：[0126][0127]因此有：[0128][0129]由于主、子惯导固连在运载体上，故认为安装误差角为常值，因此：[0130][0131]上述两式即为速度加姿态加角速度匹配传递对准的量测失准角微分方程和安装误差角微分方程。[0132]惯性器件的主要误差源有陀螺漂移ε和加速度计零偏▽。陀螺漂移主要由常值漂移ε。、相关漂移er、随机白噪声漂移^等三部分组成。相关漂移的相关时间一般大于1小时，可近似视为常值漂移，且比常值漂移小1-2个数量级，故陀螺误差模型可简化为：[0133][0134]类似的，加速度计零偏也可简化为常值漂移，即：[0135][0136]忽略垂向通道，选取的状态变量为：[0137][0138]系统状态方程为：[0139][0140]选取主、子惯导间的速度误差、量测失准角和角速度误差作为观测量，即：[0141][0142]其中，速度观测量为δVn，姿态观测量为，角速度观测量为[0143]量测方程为：[0144]Z=hX+V[0145]其中，V为系统的量测噪声。[0146]进行容积卡尔曼滤波解算，其算法流程如下：[0147]a.时间更新[0148]假设k-Ι时刻的状态xk-i的统计特性已知，先对Pk-ι做Cholesky分解：[0149][0150]计算容积点：[0151][0152]计算经系统状态方程传递后的容积点：[0153][0154]估计k时刻的状态预测值：[0155][0156]估计k时刻的状态预测协方差阵：[0157][0158]b.量测更新[0159]对Pkk-i做Cholesky分解：[0160][0161]计算容积点：[0162][0163]计算经系统量测方程传递后的容积点：[0164]Zi,kk-i=hXi,kk-ii=1,2···,2η[0165]估计k时刻的量测预测值：[0166][0167]估计k时刻的量测预测协方差阵：[0168][0169]估计k时刻的一步预测互相关协方差阵：[0170][0171]估计k时刻的滤波增益：[0172][0173]求取k时刻的状态估计值：[0174][0175]求取k时刻的状态误差协方差阵：[0176][0177]容积卡尔曼滤波器初始条件，包括状态估计协方差阵PQ、系统噪声方差阵Qo及量测噪声方差阵Ro，设定如下：[0178]P〇=diag{0.lms2,0.lms2,0.2°2,0.2°2,10°2,0.2°2,0.2°2,10°2,[0179]lXl〇-4g〇2,lX10_4g〇2,〇·〇1°Α2,0.01°h2,O.OlVh2I[0180]Q〇=diag{lX10_5g〇2,lXl〇'5g〇2,〇.〇〇l°h2,0.001°h2,0.001°h2}[0181]R〇=diag{0.1ms2,0.1ms2,0.001°2,0.001°2,0.001°2,0.5〇h2,0.5〇h2,0.5°h2}[0182]仿真结果：[0183]以上述仿真条件，仿真的到的结果如表1、图3所示。[0184]表1大方位失准角情况下安装误差角估计误差[0185；[0186]由表1和图3可以看出，米用本发明方法，纵向、横向和航向估计误差可在Is内迅速降到2角分以下，5s达到0.1角分以下，20s后估计误差达到0.01角分以下。综上所述，本发明提供的方法，可以有效地消除杆臂效应的影响，能够在舰船存在大方位失准角与大杆臂误差的情况下实现快速高精度对准。

权利要求：1.一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法，其特征在于，包括以下步骤：步骤一:完成子惯导系统的启动、预热准备；步骤二:将子惯导加速度计的比力输出转换到导航坐标系，利用巴特沃斯数字低通滤波器对其进行滤波处理，以达到消除杆臂效应误差影响的目的；步骤三:主、子惯导系统分别进行惯导解算，主惯导的速度、姿态和角速度信息传输到子惯导的导航计算机；步骤四：在舰船存在大方位失准角的情况下，采用速度加姿态加角速度的匹配方式，认为主、子惯导载体坐标系不同而导航坐标系相同，选取主、子惯导系统之间的速度误差、姿态误差和角速度误差作为观测量，建立系统的状态方程和量测方程；步骤五:利用所建立的状态方程和量测方程，进行容积卡尔曼滤波解算，估计子惯导系统与主惯导系统间的安装误差角，完成传递对准。2.根据权利要求1所述的一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法，其特征在于，子惯导系统的振荡是以舒拉周期和地球自转周期为主的振荡，处在低频区，其频谱分布在f=2XHT4Hz以下。根据杆臂效应加速度的频谱，可确定巴特沃斯低通滤波器的技术要求：通带截止频率fP=O.OIHz，通带波纹αρ=2dB，阻带截止频率fs=0.15Hz，阻带衰减〜=40dB。首先通过下式确定滤波器的阶数N。式中，代入可得N=1.80,取N=2。3dB截止频率为：由巴特沃斯归一化低通滤波器参数表可得二阶低通滤波器的归一化原型系统函数：将63p去归一化，得到模拟低通滤波器的系统函数：采样间隔T=0.05s，利用双线性变换法转换，得到数字低通滤波器的系统函数：滤波器的状态方程为：输出方程为：其中，uη表不滤波器的输入，[0.001660.70710]，d=5.53551e-06。3.根据权利要求1所述的一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法，其特征在于，忽略垂向通道，选取的状态变量为：系统状态方程为：其中，η为导航坐标系;m系为主惯导载体坐标系；s系为子惯导载体坐标系；I为子惯导计算载体坐标系;SVn为速度误差在导航坐标系的投影；为主惯导载体坐标系到子惯导载体坐标系的方向余弦矩阵；为主惯导载体坐标系到子惯导计算载体坐标系的方向余弦矩阵；为主惯导载体坐标系到导航坐标系的方向余弦矩阵；为子惯导测量的比力在其载体坐标系的投影；为地球自转角速度在导航坐标系的投影；为η系相对于地球坐标系的角速度在η系的投影为s系和m系之间的安装误差角：为系和m系之间的量测失准角；为主惯导相对于导航坐标系的角速度在m系的投影;VsS加速度计常值漂移;Wv为加速度计随机漂移;es为陀螺常值漂移：为陀螺随机漂移。采用速度加姿态加角速度的匹配方式，将主、子惯导间的速度误差SVn、量测失准角以及角速度误差作为观测量：量测方程为：Z=hX+V其中，V为系统的量测噪声。

百度查询： 哈尔滨工业大学 一种基于容积卡尔曼滤波的舰船大方位失准角传递对准方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD