CN115685193A - 一种基于雷达测距的发射坐标系组合导航方法 - Google Patents

一种基于雷达测距的发射坐标系组合导航方法 Download PDF

Info

Publication number
CN115685193A
CN115685193A CN202211425680.2A CN202211425680A CN115685193A CN 115685193 A CN115685193 A CN 115685193A CN 202211425680 A CN202211425680 A CN 202211425680A CN 115685193 A CN115685193 A CN 115685193A
Authority
CN
China
Prior art keywords
coordinate system
radar
guided
matrix
time
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211425680.2A
Other languages
English (en)
Inventor
陈凯
房琰
梁文超
曾诚之
杨睿华
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202211425680.2A priority Critical patent/CN115685193A/zh
Publication of CN115685193A publication Critical patent/CN115685193A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明公开了一种基于雷达测距的发射坐标系组合导航方法，涉及制导炮弹组合导航领域，在卫星拒止环境下使用雷达进行测距，通过对雷达数据的处理，得到发射坐标系下的位置信息，并使用得到的位置信息进行组合导航。

Description

一种基于雷达测距的发射坐标系组合导航方法
技术领域
本发明涉及制导炮弹组合导航领域，具体涉及一种基于雷达测距的发射坐标系组合导航方法。
背景技术
惯性导航系统具有完全自主性，但其定位误差会随着时间的积累而逐渐加大。组合导航技术通过其他导航系统修正惯性导航系统的误差，可以提高导航精度。惯导/卫星组合导航是目前常用的组合导航方式，卫星导航系统的精度较高，且误差不随时间积累。但在卫星拒止环境下，卫星接收机难以提供导航信息，无法校正惯性导航系统。因此，研究制导炮弹组合导航时，实现卫星拒止环境下的快速组合导航是该领域的技术难点。
针对卫星拒止环境，一些制导炮弹使用地磁导航系统，但是磁力计误差较大，易受外界环境干扰，精度较低。为了解决卫星拒止环境下的组合导航问题，同时又保证方法的实时性，本发明提出的一种基于雷达测距的发射坐标系组合导航方法，是使用雷达进行测距，通过对雷达数据的处理，得到发射坐标系下的位置信息，并使用得到的位置信息进行组合导航。
发明内容
针对现有技术中的上述不足，本发明提供了一种基于雷达测距的发射坐标系组合导航方法。
为了达到上述发明目的，本发明采用的技术方案为：
一种基于雷达测距的发射坐标系组合导航方法，包括如下步骤：
S1、制导炮弹升空完成姿态辨识之后，根据制导炮弹的初始装订信息、雷达装订信息以及雷达测量值计算制导炮弹在发射坐标系下的位置信息和速度信息；
S2、对制导炮弹的状态方程和量测方程进行离散化；
S3、利用滤波递推算法计算制导炮弹下一时刻的状态，并重复滤波直至收敛状态得到姿态、速度、位置、陀螺仪漂移和加速度计漂移的估算误差值；
S4、将计算得到的估算误差值修正至导航解算值中，完成制导炮弹的组合导航。
进一步的，所述S1具体包括如下步骤：
S11、根据制导炮弹的初始装订信息计算地心地固坐标系到发射坐标系的方向余弦矩阵和发射点的空间直角坐标，同时根据雷达装订信息计算雷达直角坐标系到地心地固坐标系的方向余弦矩阵和雷达的空间直角坐标；
S12、根据雷达测量的方位角、高低角、距离计算雷达直角坐标系下的制导炮弹的位置信息；
S13、根据雷达直角坐标系到地心地固坐标系的转换矩阵、雷达空间直角坐标以及雷达直角坐标系下制导炮弹的位置信息计算制导炮弹的大地坐标；
S14、根据地心地固坐标系到发射坐标系的转换矩阵、制导炮弹发射点的空间直角坐标以及制导炮弹的大地坐标计算制导炮弹发射坐标系下的位置信息；
S15、根据制导炮弹发射坐标系下的位置信息计算制导炮弹的速度信息。
进一步的，所述S13中计算制导炮弹的大地坐标的计算公式为：
Figure BDA0003944542450000021
其中，Pe_p为制导炮弹的大地坐标，Pe1为雷达的空间直角坐标，
Figure BDA0003944542450000022
为雷达直角坐标系到地心地固坐标系的转换矩阵，
Figure BDA0003944542450000023
为雷达直角坐标系下制导炮弹的位置信息。
进一步的，所述S14中计算制导炮弹发射坐标系下的位置信息的计算公式为：
Figure BDA0003944542450000031
其中，Pg_p为制导炮弹发射坐标系下的位置信息，
Figure BDA0003944542450000032
为地心地固坐标系到发射坐标系的转换矩阵，Pe_p为制导炮弹的大地坐标，Pe0为制导炮弹发射点空间直角坐标。
进一步的，所述S15中计算制导炮弹的速度的具体方式为：
根据ta时刻的位置信息和tb时刻的位置信息求解ta时刻制导炮弹的速度信息，计算公式为：
Figure BDA0003944542450000033
其中，
Figure BDA0003944542450000034
为ta时刻制导炮弹发射坐标系下的速度信息，
Figure BDA0003944542450000035
为ta时刻制导炮弹发射坐标系下的位置信息，
Figure BDA0003944542450000036
为tb时刻制导炮弹发射坐标系下的位置信息，且tb<ta。
进一步的，所述S2中状态方程和量测方程的离散化分别表示为：
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1
Zk＝HkXk+Vk
其中，Xk是k时刻的状态矢量，Xk-1是k-1时刻的状态矢量，Wk-1是k-1时刻的过程噪声矢量，Zk为k时刻的位置量测矢量，Hk为k时刻的观测矩阵，Vk为k时刻的量测噪声向量，Φk/k-1与Γk/k-1为状态方程和噪声驱动矩阵的离散化。
Figure BDA0003944542450000037
Figure BDA0003944542450000041
其中，F(tn)为tn时刻的状态转移矩阵，F(tk)和G(tk)为tk时刻的状态转移矩阵和噪声驱动矩阵，T为离散化的周期，n为离散化的阶数。
进一步的，所述S3中利用滤波递推算法计算制导炮弹下一时刻的状态具体方式为：
S31、根据初始状态进行状态一步预测
Figure BDA0003944542450000042
Figure BDA0003944542450000043
其中，
Figure BDA0003944542450000044
为k-1时刻状态矢量的收敛值；
S32、利用一步预测值进行状态估计：
Figure BDA0003944542450000045
其中，Kk为滤波增益矩阵；
Figure BDA0003944542450000046
其中，Rk为k时刻量测噪声的协方差阵，Pk/k-1为一步预测均方误差阵，其对角线元素是各个状态估计的方差；
Figure BDA0003944542450000047
其中，Qk-1为k-1时刻系统噪声的协方差阵，Pk-1为k-1时刻估计均方误差矩阵，k时刻估计均方误差矩阵Pk为
Figure BDA0003944542450000048
S33、给定初值X0和P0，根据k时刻的位置量测矢量Zk，递推求得k时刻的状态估计
Figure BDA0003944542450000049
S34、经过重复计算n次得到
Figure BDA00039445424500000410
的收敛值，即姿态误差值
Figure BDA00039445424500000411
速度误差值
Figure BDA00039445424500000412
位置误差值
Figure BDA0003944542450000051
陀螺漂移误差值
Figure BDA0003944542450000052
加速度计漂移误差值
Figure BDA0003944542450000053
进一步的，所述S3中估算误差值包括制导炮弹的姿态、速度、位置、陀螺漂移和加速度计漂移的估算误差值。
进一步的，所述S4具体包括：
S41、通过导航算法计算得到姿态信息
Figure BDA0003944542450000054
速度信息
Figure BDA0003944542450000055
和速度信息
Figure BDA0003944542450000056
S42、通过S3得到的估算误差值分别对姿态信息、速度和位置进行修正，完成组合导航。
进一步的，所述S42中对姿态信息的修正方式为：
Figure BDA0003944542450000057
其中，
Figure BDA0003944542450000058
为计算得到的姿态四元数，
Figure BDA0003944542450000059
为校正后的姿态四元数，Qk为姿态误差
Figure BDA00039445424500000510
对应的姿态四元数；
对速度的修正方式为：
Figure BDA00039445424500000511
其中，Vg为修正后的速度，
Figure BDA00039445424500000512
为计算得到的速度，
Figure BDA00039445424500000513
为速度估算误差值；
对位置的修正方式为：
Figure BDA00039445424500000514
其中，Pg为修正后的位置，
Figure BDA00039445424500000515
为计算得到的位置，
Figure BDA00039445424500000516
为位置估算误差值。
本发明具有以下有益效果：
使用雷达进行测距，通过对雷达数据的处理，得到发射坐标系下的位置信息，并使用得到的位置信息进行组合导航，实现了在卫星拒止环境下的组合导航。
附图说明
图1为本发明基于雷达测距的发射坐标系组合导航方法流程示意图。
图2为本发明实施例中雷达直角坐标系与发射坐标系的关系示意图。
图3为本发明实施例中雷达极坐标系与雷达直角坐标系的关系示意图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
为了实现上述目的，本发明提出以下方案：
本发明使用到以下坐标系及坐标系转换
1)地心地固坐标系(e系)
地心地固坐标系(Earth-Centered Earth-Fixed frame)，坐标系原点Oe为地球中心，xe轴在赤道平面内并且指向本初子午线，ze轴为地球自转轴并且指向北极，ye轴在赤道平面内并且与xe轴、ze轴构成右手直角坐标系。e系中有两种坐标类型，分别是空间直角坐标系和大地坐标系。空间直角坐标系使用矢量[xe,ye,ze]T表示一个点的位置。大地坐标系用(B,λ,h)来表示一个点的位置，B、λ、h分别为纬度、经度、高度。
2)发射坐标系(g系)
发射坐标系(Launch-Centered Earth-Fixed frame)，坐标系原点Og为发射点，xg轴在发射点水平面内并且指向发射瞄准方向，yg轴垂直于发射点水平面并且指向上方，zg轴与xg轴、yg轴构成右手直角坐标系，发射坐标系与地球固连。发射点的地理纬度B0、经度λ0、高度h0和发射方位角A0确定了发射坐标系与地球之间的关系。
3)雷达测量坐标系(m系)
测量坐标系有两种类型，分别是雷达极坐标系和雷达直角坐标系。雷达直角坐标系原点Om为雷达，xm轴在雷达水平面内并且指向设定的基准方向，ym轴垂直于雷达水平面并且指向上方，zm轴与xm轴、ym轴构成右手直角坐标系，雷达直角坐标系与地球固连。雷达的地理纬度B1、经度λ1、高度h1和大地方位角A1确定了雷达直角坐标系与地球之间的关系，雷达直角坐标系与发射坐标系如图2所示。
雷达极坐标系以距离R、高低角E以及方位角A确定空间中某点的位置，雷达极坐标系与雷达直角坐标系的关系如图3所示。目标T的雷达直角坐标T(xm,ym,zm)通过式(1)由雷达极坐标T(A,E,R)转换得到。
Figure BDA0003944542450000071
4)弹体坐标系(b系)
弹体坐标系(Body frame)，坐标系原点Ob为弹体质心，xb轴沿弹体纵轴指向弹体正前，yb轴在弹体主对称轴平面内指向弹体上方，zb轴与xb轴、yb轴构成右手直角坐标系指向弹体右方。
5)旋转矩阵
绕x轴旋转α的旋转矩阵为
Figure BDA0003944542450000072
绕y轴旋转α的旋转矩阵为
Figure BDA0003944542450000081
绕z轴旋转α的旋转矩阵为
Figure BDA0003944542450000082
6)大地坐标向空间直角坐标的转换关系
卯酉圈曲率半径RN沿东西方向定义，计算公式为
Figure BDA0003944542450000083
式中，a为地球半长轴，e为地球偏心率。
大地坐标向空间直角坐标的转换关系如式(6)所示
Figure BDA0003944542450000084
7)发射坐标系与地心地固坐标系
发射坐标系到地心地固坐标系的方向余弦矩阵为
Figure BDA0003944542450000085
地心地固坐标系旋转到发射坐标系，由3次旋转获得，涉及制导炮弹初始经度λ0、地理纬度B0和发射方位角A0，地心地固坐标系到发射坐标系的转换矩阵
Figure BDA0003944542450000086
为：
Figure BDA0003944542450000087
发射坐标系到地心地固坐标系的转换矩阵
Figure BDA0003944542450000091
为
Figure BDA0003944542450000092
8)雷达直角坐标系与地心地固坐标系
雷达直角坐标系到地心地固坐标系的方向余弦矩阵为
Figure BDA0003944542450000093
地心地固坐标系旋转到雷达直角坐标系，由3次旋转获得，涉及雷达的经度λ1、地理纬度B1和大地方位角A1，地心地固坐标系到发射坐标系的转换矩阵
Figure BDA0003944542450000094
为：
Figure BDA0003944542450000095
雷达直角坐标系到地心地固坐标系的转换矩阵
Figure BDA0003944542450000096
为
Figure BDA0003944542450000097
9)发射坐标系与弹体坐标系之间的转换关系
制导炮弹在发射坐标系相对弹体坐标系的姿态角由俯仰角
Figure BDA0003944542450000098
偏航角ψ和滚转角γ三个欧拉角描述，发射坐标系到弹体坐标系，按照先绕z轴旋转俯仰角
Figure BDA0003944542450000099
再绕y轴旋转偏航角ψ、后绕x轴旋转滚转角γ的3-2-1旋转顺序，得转换矩阵
Figure BDA00039445424500000910
如下：
Figure BDA00039445424500000911
弹体坐标系到发射坐标系的转换矩阵
Figure BDA00039445424500000912
为
Figure BDA00039445424500000913
基于上述原理，本发明提供一种基于雷达测距的发射坐标系组合导航方法，如图1所示，包括如下步骤：
S1、制导炮弹升空完成姿态辨识之后获取制导炮弹的初始装订信息以及雷达检测到的炮弹装订信息，根据雷达检测到的炮弹装填信息计算制导炮弹在发射坐标系下的位置信息和速度信息；
具体过程为：
S11、根据制导炮弹的初始装订信息计算地心地固坐标系到发射坐标系的方向余弦矩阵和发射点的空间直角坐标，同时根据雷达装订信息计算雷达直角坐标系到地心地固坐标系的方向余弦矩阵和雷达的空间直角坐标；
在本实施例里，将导炮弹初始经度λ0、地理纬度B0和发射方位角A0代入式(7)得到
Figure BDA0003944542450000101
将
Figure BDA0003944542450000102
代入公式(8)得到
Figure BDA0003944542450000103
将发射点大地坐标LLH0＝(B0,λ0,h0)代入式(6)，得到发射点空间直角坐标Pe0＝(xe0,ye0,ze0)。
将雷达检测到的经度λ1、地理纬度B1和大地方位角A1代入式(9)得到
Figure BDA0003944542450000104
将
Figure BDA0003944542450000105
代入(10)得到
Figure BDA0003944542450000106
将雷达大地坐标LLH1＝(B1,λ1,h1)代入式(6)，得到雷达空间直角坐标Pe1＝(xe1,ye1,ze1)。
S12、根据雷达测量的方位角、高低角、距离计算雷达直角坐标系下的制导炮弹的位置信息；
本实施例里，将雷达测量的方位角
Figure BDA0003944542450000107
高低角
Figure BDA0003944542450000108
距离
Figure BDA0003944542450000109
代入式(1)，可以得到雷达直角坐标系下制导炮弹的位置信息
Figure BDA00039445424500001010
S13、根据雷达直角坐标系到地心地固坐标系的转换矩阵、雷达空间直角坐标以及雷达直角坐标系下制导炮弹的位置信息计算制导炮弹的大地坐标；
本实施例里，将
Figure BDA00039445424500001011
代入式(13)，得到制导炮弹的大地坐标Pe_p＝(xe_p,ye_p,ze_p).
Figure BDA0003944542450000111
式(13)中，Pe_p为制导炮弹的大地坐标，Pe1为制导炮弹的雷达空间直角坐标，
Figure BDA0003944542450000112
为雷达直角坐标系到地心地固坐标系的转换矩阵，
Figure BDA0003944542450000113
为雷达直角坐标系下制导炮弹的位置信息。
S14、根据发射坐标系到地心地固坐标系的转换矩阵、制导炮弹的发射点空间直角坐标以及制导炮弹的大地坐标计算制导炮弹发射坐标系下的位置信息；
本实施例，将Pe_p代入式(14)，得到制导炮弹发射坐标系下的位置信息Pg_p＝(xg_p,yg_p,zg_p)
Figure BDA0003944542450000114
式(14)中，Pg_p为制导炮弹发射坐标系下的位置信息，
Figure BDA0003944542450000115
为地心地固坐标系到发射坐标系的转换矩阵，Pe_p为制导炮弹的大地坐标，Pe0为制导炮弹发射点空间直角坐标。
S15、根据制导炮弹发射坐标系下的位置信息计算制导炮弹的速度信息。
本实施例里，记ta时刻的制导炮弹位置信息为
Figure BDA0003944542450000116
记tb(tb<ta)时刻的制导炮弹位置信息为
Figure BDA0003944542450000117
则ta时刻的制导炮弹速度信息
Figure BDA0003944542450000118
可表示为
Figure BDA0003944542450000119
S2、对制导炮弹的状态方程和量测方程进行离散化；
发射坐标系惯导/雷达组合导航方法的卡尔曼滤波状态方程为
Figure BDA00039445424500001110
式(16)中，X为状态矢量，以15维状态矢量为例，状态矢量包括发射坐标系下的姿态误差矢量φg、速度误差矢量δVg、位置误差矢量δPg、陀螺仪误差矢量δεb和加速度计误差矢量
Figure BDA0003944542450000121
如式(17)所示
Figure BDA0003944542450000122
F为状态转移矩阵，如式(18)所示
Figure BDA0003944542450000123
式(18)中，
Figure BDA0003944542450000124
为地球自转角速度在发射坐标系下的投影的反对称矩阵，Fg为加速度计测量值在发射坐标系下的投影的反对称矩阵，GP是由位置误差引起的标准重力误差在发射坐标系下的投影。
式(16)中，G为噪声驱动矩阵，如式(19)所示
Figure BDA0003944542450000125
式(16)中，W为过程噪声矢量，包括陀螺仪白噪声wg和加速度计白噪声wa，如式(20)所示
Figure BDA0003944542450000126
发射坐标系下捷联惯导的位置可表示为
PI＝Pt+δPI (21)
式(21)中，PI为惯导输出的位置，δPI为惯导输出位置时相应的误差，Pt为制导炮弹位置真值。
由于雷达测距的精度高，而雷达测速是通过位置信息差分实现的，速度精度较低，因此组合导航以位置为观测量，雷达导航的位置可表示为
Pg_p＝Pt+δPg_p (22)
式(22)中，Pg_p为雷达导航输出位置，δPg_p为雷达导航输出位置时相应的误差。
位置量测矢量为
Zp＝PI-Pg_p (23)
发射坐标系惯导/雷达组合导航方法的卡尔曼滤波量测方程为
Zp＝HpX+Vp (24)
式(24)中观测矩阵Hp的表达式如所示，量测噪声向量Vp为位置输出误差。
Hp＝[03×3 03×3 I3×3 03×3 03×3] (25)
基于此，状态方程和量测方程的离散化分别为
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1
Zk＝HkXk+Vk
其中，Xk是k时刻的状态矢量，Xk-1是k-1时刻的状态矢量，Wk-1是k-1时刻的过程噪声矢量，Zk为k时刻的位置量测矢量，Hk为k时刻的观测矩阵，Vk为k时刻的量测噪声向量，Φk/k-1与Γk/k-1为状态方程和噪声驱动矩阵的离散化。
Figure BDA0003944542450000131
Figure BDA0003944542450000132
其中，F(tn)为tn时刻的状态转移矩阵，F(tk)和G(tk)为tk时刻的状态转移矩阵和噪声驱动矩阵，T为离散化的周期，n为离散化的阶数。
S3、利用滤波递推算法计算制导炮弹下一时刻的状态，并重复滤波直至收敛状态得到姿态、速度、位置、陀螺仪漂移和加速度计漂移的估算误差值；
在本实施例里，具体方式为：
S31、根据初始状态进行状态一步预测
Figure BDA0003944542450000141
Figure BDA0003944542450000142
其中，
Figure BDA0003944542450000143
为k-1时刻状态矢量的收敛值；
S32、利用一步预测值进行状态估计：
Figure BDA0003944542450000144
其中，Kk为滤波增益矩阵；
Figure BDA0003944542450000145
其中，Rk为k时刻量测噪声的协方差阵，Pk/k-1为一步预测均方误差阵，其对角线元素是各个状态估计的方差；
Figure BDA0003944542450000146
其中，Qk-1为k-1时刻系统噪声的协方差阵，Pk-1为k-1时刻估计均方误差矩阵，k时刻估计均方误差矩阵Pk为
Figure BDA0003944542450000147
S33、给定初值X0和P0，根据k时刻的位置量测矢量Zk，递推求得k时刻的状态估计
Figure BDA0003944542450000148
S34、经过重复计算n次得到
Figure BDA0003944542450000149
的收敛值，即姿态误差值
Figure BDA00039445424500001410
速度误差值
Figure BDA00039445424500001411
位置误差值
Figure BDA00039445424500001412
陀螺漂移误差值
Figure BDA00039445424500001413
加速度计漂移误差值
Figure BDA00039445424500001414
S4、将计算得到的估算误差值修正至导航解算值中，完成制导炮弹的组合导航。
通过导航算法计算得到姿态信息
Figure BDA0003944542450000151
速度信息
Figure BDA0003944542450000152
位置信息
Figure BDA0003944542450000153
(1)姿态修正：
由四元数计算
Figure BDA0003944542450000154
其中，
Figure BDA0003944542450000155
为计算得到的姿态四元数，
Figure BDA0003944542450000156
为校正后的姿态四元数，Qk为姿态误差
Figure BDA0003944542450000157
对应的四元数。
(2)对速度的修正方式为：
Figure BDA0003944542450000158
其中，Vg为修正后的速度，
Figure BDA0003944542450000159
为计算得到的速度，
Figure BDA00039445424500001510
为速度估算误差值；
(3)对位置的修正方式为：
Figure BDA00039445424500001511
其中，Pg为修正后的位置，
Figure BDA00039445424500001512
为计算得到的位置，
Figure BDA00039445424500001513
为位置估算误差值。
本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (10)

1.一种基于雷达测距的发射坐标系组合导航方法，其特征在于，包括如下步骤：
S1、制导炮弹升空完成姿态辨识之后，根据制导炮弹的初始装订信息、雷达装订信息以及雷达测量值计算制导炮弹在发射坐标系下的位置信息和速度信息；
S2、对制导炮弹的状态方程和量测方程进行离散化；
S3、利用滤波递推算法计算制导炮弹下一时刻的状态，并重复滤波直至收敛状态得到姿态、速度、位置、陀螺仪漂移和加速度计漂移的估算误差值；
S4、将计算得到的估算误差值修正至导航解算值中，完成制导炮弹的组合导航。
2.根据权利要求1所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S1具体包括如下步骤：
S11、根据制导炮弹的初始装订信息计算地心地固坐标系到发射坐标系的方向余弦矩阵和发射点的空间直角坐标，同时根据雷达装订信息计算雷达直角坐标系到地心地固坐标系的方向余弦矩阵和雷达的空间直角坐标；
S12、根据雷达测量的方位角、高低角、距离计算雷达直角坐标系下的制导炮弹的位置信息；
S13、根据雷达直角坐标系到地心地固坐标系的转换矩阵、雷达空间直角坐标以及雷达直角坐标系下制导炮弹的位置信息计算制导炮弹的大地坐标；
S14、根据地心地固坐标系到发射坐标系的转换矩阵、制导炮弹发射点的空间直角坐标以及制导炮弹的大地坐标计算制导炮弹发射坐标系下的位置信息；
S15、根据制导炮弹发射坐标系下的位置信息计算制导炮弹的速度信息。
3.根据权利要求2所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S13中计算制导炮弹的大地坐标的计算公式为：
Figure FDA0003944542440000021
其中，Pe_p为制导炮弹的大地坐标，Pe1为雷达的空间直角坐标，
Figure FDA0003944542440000022
为雷达直角坐标系到地心地固坐标系的转换矩阵，
Figure FDA0003944542440000023
为雷达直角坐标系下制导炮弹的位置信息。
4.根据权利要求2所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S14中计算制导炮弹发射坐标系下的位置信息的计算公式为：
Figure FDA0003944542440000024
其中，Pg_p为制导炮弹发射坐标系下的位置信息，
Figure FDA0003944542440000025
为地心地固坐标系到发射坐标系的转换矩阵，Pe_p为制导炮弹的大地坐标，Pe0为制导炮弹发射点空间直角坐标。
5.根据权利要求2所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S15中计算制导炮弹的速度的具体方式为：
根据ta时刻的位置信息和tb时刻的位置信息求解ta时刻制导炮弹的速度信息，计算公式为：
Figure FDA0003944542440000026
其中，Vg_p_ta为ta时刻制导炮弹发射坐标系下的速度信息，Pg_p_ta为ta时刻制导炮弹发射坐标系下的位置信息，Pg_p_tb为tb时刻制导炮弹发射坐标系下的位置信息，且tb<ta。
6.根据权利要求1所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S2中状态方程和量测方程的离散化分别表示为：
Xk＝Φk/k-1Xk-1+Γk/k-1Wk-1
Zk＝HkXk+Vk
其中，Xk是k时刻的状态矢量，Xk-1是k-1时刻的状态矢量，Wk-1是k-1时刻的过程噪声矢量，Zk为k时刻的位置量测矢量，Hk为k时刻的观测矩阵，Vk为k时刻的量测噪声向量，Φk/k-1与Γk/k-1为状态方程和噪声驱动矩阵的离散化；
Figure FDA0003944542440000031
Figure FDA0003944542440000032
其中，F(tn)为tn时刻的状态转移矩阵，F(tk)和G(tk)为tk时刻的状态转移矩阵和噪声驱动矩阵，T为离散化的周期，n为离散化的阶数。
7.根据权利要求1所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S3中利用滤波递推算法计算制导炮弹下一时刻的状态具体方式为：
S31、根据初始状态进行状态一步预测
Figure FDA0003944542440000033
Figure FDA0003944542440000034
其中，
Figure FDA0003944542440000035
为k-1时刻状态矢量的收敛值；
S32、利用一步预测值进行状态估计：
Figure FDA0003944542440000036
其中，Kk为滤波增益矩阵；
Figure FDA0003944542440000037
其中，Rk为k时刻量测噪声的协方差阵，Pk/k-1为一步预测均方误差阵，其对角线元素是各个状态估计的方差；
Figure FDA0003944542440000041
其中，Qk-1为k-1时刻系统噪声的协方差阵，Pk-1为k-1时刻估计均方误差矩阵，k时刻估计均方误差矩阵Pk为
Figure FDA0003944542440000042
S33、给定初值X0和P0，根据k时刻的位置量测矢量Zk，递推求得k时刻的状态估计
Figure FDA0003944542440000043
S34、经过重复计算n次得到
Figure FDA0003944542440000044
的收敛值，即姿态误差值
Figure FDA0003944542440000045
速度误差值
Figure FDA0003944542440000046
位置误差值
Figure FDA0003944542440000047
陀螺漂移误差值
Figure FDA0003944542440000048
加速度计漂移误差值
Figure FDA0003944542440000049
8.根据权利要求1所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S3中估算误差值包括制导炮弹的姿态、速度、位置、陀螺漂移和加速度计漂移的估算误差值。
9.根据权利要求1所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S4具体包括：
S41、通过导航算法计算得到姿态信息
Figure FDA00039445424400000410
速度信息
Figure FDA00039445424400000411
和速度信息
Figure FDA00039445424400000412
S42、通过S3得到的估算误差值分别对姿态信息、速度和位置进行修正，完成组合导航。
10.根据权利要求9所述的基于雷达测距的发射坐标系组合导航方法，其特征在于，所述S42中对姿态信息的修正方式为：
Figure FDA00039445424400000413
其中，
Figure FDA00039445424400000414
为计算得到的姿态四元数，
Figure FDA00039445424400000415
为校正后的姿态四元数，Qk为姿态误差
Figure FDA0003944542440000051
对应的姿态四元数；
对速度的修正方式为：
Figure FDA0003944542440000052
其中，Vg为修正后的速度，
Figure FDA0003944542440000053
为计算得到的速度，
Figure FDA0003944542440000054
为速度估算误差值；
对位置的修正方式为：
Figure FDA0003944542440000055
其中，Pg为修正后的位置，
Figure FDA0003944542440000056
为计算得到的位置，
Figure FDA0003944542440000057
为位置估算误差值。
CN202211425680.2A 2022-11-15 2022-11-15 一种基于雷达测距的发射坐标系组合导航方法 Pending CN115685193A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211425680.2A CN115685193A (zh) 2022-11-15 2022-11-15 一种基于雷达测距的发射坐标系组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211425680.2A CN115685193A (zh) 2022-11-15 2022-11-15 一种基于雷达测距的发射坐标系组合导航方法

Publications (1)

Publication Number Publication Date
CN115685193A true CN115685193A (zh) 2023-02-03

Family

ID=85051305

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211425680.2A Pending CN115685193A (zh) 2022-11-15 2022-11-15 一种基于雷达测距的发射坐标系组合导航方法

Country Status (1)

Country Link
CN (1) CN115685193A (zh)

Similar Documents

Publication Publication Date Title
CN110926468B (zh) 基于传递对准的动中通天线多平台航姿确定方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
US7133776B2 (en) Attitude alignment of a slave inertial measurement system
CN101793523A (zh) 一种组合导航和光电探测一体化系统
CN110243362B (zh) 一种中高空超声速靶标导航方法
CN111141273A (zh) 基于多传感器融合的组合导航方法及系统
CN103604428A (zh) 基于高精度水平基准的星敏感器定位方法
CN114993305A (zh) 一种基于发射坐标系的制导炮弹组合导航方法
CN113503892B (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN112432642A (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN110514200B (zh) 一种惯性导航系统及高转速旋转体姿态测量方法
CN116105730A (zh) 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN108151765A (zh) 一种在线实时估计补偿磁强计误差的定位测姿方法
US9217639B1 (en) North-finding using inertial navigation system
CN110332933A (zh) 车辆定位方法、终端及计算机可读存储介质
CN112833878A (zh) 近地面多源天文自主导航方法
CN115727846B (zh) 一种捷联惯导与卫导的多线程回溯式组合导航方法
US10222214B2 (en) Digital sight for hand-carried projectile-firing device and method of controlling the same
CN114674313B (zh) 一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法
CN115479605A (zh) 基于空间目标定向观测的高空长航时无人机自主导航方法
CN115685193A (zh) 一种基于雷达测距的发射坐标系组合导航方法
CN115950419A (zh) 超小型无人机用组合导航方法、装置及系统
CN114353784A (zh) 一种基于运动矢量的制导炮弹空中姿态辨识方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination