CN113686333B - 一种针对雷达阵面旋转的捷联惯导全姿态表示方法 - Google Patents

一种针对雷达阵面旋转的捷联惯导全姿态表示方法 Download PDF

Info

Publication number
CN113686333B
CN113686333B CN202111079664.8A CN202111079664A CN113686333B CN 113686333 B CN113686333 B CN 113686333B CN 202111079664 A CN202111079664 A CN 202111079664A CN 113686333 B CN113686333 B CN 113686333B
Authority
CN
China
Prior art keywords
angle
coordinate system
attitude
quaternion
array surface
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.)
Active
Application number
CN202111079664.8A
Other languages
English (en)
Other versions
CN113686333A (zh
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.)
707th Research Institute of CSIC
Original Assignee
707th Research Institute of CSIC
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 707th Research Institute of CSIC filed Critical 707th Research Institute of CSIC
Priority to CN202111079664.8A priority Critical patent/CN113686333B/zh
Publication of CN113686333A publication Critical patent/CN113686333A/zh
Application granted granted Critical
Publication of CN113686333B publication Critical patent/CN113686333B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明涉及一种针对雷达阵面旋转的捷联惯导全姿态表示方法，步骤为：1、建立初始条件：惯导固定于雷达车的雷达阵面上，设定不存在安装误差，将雷达阵面转动至直立位，设定该位置为雷达阵面俯仰角真值0°附近；2、惯导上电后开始对准，对准完成后，得到雷达阵面在俯仰角0°附近的惯导姿态对应的***四元数、***方向余弦矩阵、***姿态角；3、绕阵面旋转中心点转动雷达阵面，在转动的过程中利用捷联惯导数值递推姿态更新算法，进行***四元数和***方向余弦矩阵的迭代更新，同时更新过程量***姿态角；并实时将***俯仰角与45°阈值进行比较，以确定用直立坐标系还是倒伏坐标系作为惯导载体坐标系，然后计算输出姿态角。本发明保证了输出姿态角的连续性和准确性。

Description

一种针对雷达阵面旋转的捷联惯导全姿态表示方法
技术领域
本发明属于捷联惯性导航设备全姿态提取领域，具体涉及一种针对雷达阵面旋转的捷联惯导全姿态表示方法。
背景技术
姿态角是一种最直观、最容易理解的姿态表示方法，广泛应用于海陆空天领域。某类型雷达车的雷达阵面俯仰角的工作范围是0°至90°，通常情况下，雷达阵面会在液压杆的起竖作用下，由倒伏状态切换至直立状态或者由直立状态切换至倒伏状态，即俯仰角在0°至90°间动态变化，见图1。惯导安装在雷达阵面上，设定不存在安装误差，那么惯导俯仰角也将在0°至90°间动态变化。在雷达阵面运动过程中，需要惯导实时准确地输出姿态角来反映当前的雷达阵面姿态。然而，常规的惯导姿态解算方案中，俯仰角在90°附近时，由于欧拉角奇异性，横滚角和方位角两者之间是无法单独分离的，或者说两者都存在多值性，这会引起较大的姿态角提取误差，使得雷达阵面无法获取准确的横滚角和方位角，进而影响雷达车的正常工作。
发明内容
本发明的目的在于克服现有技术的不足之处，提供一种当雷达阵面俯仰角在0°至90°间动态变化时，通过惯导能实时准确的提取姿态角，以能准确反映雷达阵面姿态的针对雷达阵面旋转的捷联惯导全姿态表示方法。
本发明的目的通过如下技术方案来实现：
一种针对雷达阵面旋转的捷联惯导全姿态表示方法，其特征在于：包括如下步骤：
步骤1、建立初始条件：惯导固定于雷达车的雷达阵面上，设定不存在安装误差，将雷达阵面转动至直立位，设定该位置为雷达阵面俯仰角真值0°位置附件；在惯导的控制***内预设有两个载体坐标系，一个为直立坐标系，另一个为倒伏坐标系；直立坐标系定义为：雷达阵面俯仰角真值0°位置时，坐标系原点位于惯导质心，x轴指向雷达车左侧，y轴指向雷达车尾，z轴指向天，倒伏坐标系的定义为：依据右手定则，实时由当前的直立坐标系绕x轴按x轴的负方向旋转90°获得；
步骤2、惯导上电后开始对准，雷达阵面维持几分钟不动，对准完成后，得到雷达阵面在俯仰角0°附近时惯导姿态对应的***四元数、***方向余弦矩阵、***姿态角；
步骤3、绕阵面旋转中心点转动雷达阵面，在转动的过程中利用捷联惯导数值递推姿态更新算法，进行***四元数和***方向余弦矩阵的迭代更新，同时更新过程量***姿态角；并在该过程中，实时将***俯仰角与阈值45°比较；若***俯仰角小于45°，则处于直立状态，则在直立坐标系下，将***四元数赋值输出四元数，然后利用四元数与姿态角间转换关系，计算得到输出姿态角，输出姿态角包括输出横滚角、输出方位角和输出俯仰角三个参量，若***俯仰角大于等于45°，则处于倒伏状态，则利用转换四元数将惯导载体坐标系由当前直立坐标系依据右手定则绕x轴按x轴的负方向旋转90°得到倒伏坐标系，在倒伏坐标系下计算得到输出姿态角。
进一步的：当惯导载体坐标系由直立坐标系切换至倒伏坐标系后，计算输出的横滚角和方位角均与在直立坐标系下的一致，俯仰角相比直立坐标系存在-90°的固定差。
输出姿态角的计算过程如下：
1)计算转换四元数：记横滚角为γ，俯仰角为θ，方位角为Ψ，四元数为q，则姿态角与四元数转换关系：
Figure BDA0003263511350000021
直立坐标系下t时刻雷达阵面的姿态四元数为Qt，倒伏坐标系下t时刻雷达阵面的姿态四元数为qt。直立坐标系至倒伏坐标系的转换角度为俯仰角-90°，依据上面的姿态角与四元数转换关系得到对应该转换角度的转换四元数为：
Figure BDA0003263511350000022
那么存在下述关系：
qt＝Qt*q
其中，*代表四元数乘法运算；
3)计算输出姿态角
定义t时刻输出四元数为：
Figure BDA0003263511350000031
则在直立状态下，
Figure BDA0003263511350000034
在倒伏状态下，/>
Figure BDA0003263511350000035
利用输出四元数提取输出姿态角：
θ＝arcsin(2(q2q3+q0q1))
Figure BDA0003263511350000032
Figure BDA0003263511350000033
本发明具有的优点和积极效果为：
本发明在雷达阵面运动过程中，不需要外部信息指示，即可实时自主判定当前状态，根据当前状态下惯导的***俯仰角与45°阈值的比较结果，确定是在直立坐标系下输出姿态角，还是在倒伏坐标系下输出姿态角，该捷联惯导全姿态表示方法，避免了常规的惯导姿态解算方案中，俯仰角在90°附近时，由于欧拉角奇异性，导致横滚角和方位角两者之间无法单独分离，或者说两者存在多值性，而引起较大的欧拉角提取误差的问题，保证了输出姿态角的连续性和准确性。
附图说明
图1是本发明雷达阵面旋转示意图；
图2是本发明倒伏状态下直立坐标系的结构示意图；
图3是本发明倒伏状态下倒伏坐标系的结构示意图；
图4是本发明捷联惯导全姿态表示方法的流程图。
具体实施方式
以下结合附图并通过实施例对本发明的结构作进一步说明。需要说明的是本实施例是叙述性的，而不是限定性的。
一种针对雷达阵面旋转的捷联惯导全姿态表示方法，主要发明点为：姿态状态自主判定，四元数转换坐标系。
工作流程见图4，具体如下：
(1)将惯导固定于雷达车的雷达阵面上，设定不存在安装误差，将雷达阵面转动至直立位，设定该位置雷达阵面俯仰角真值在0°附近；
(2)惯导上电后开始对准，雷达阵面维持不动大约5分钟，对准完成后，得到雷达阵面在俯仰角0°附近的惯导姿态对应的***四元数、***方向余弦矩阵、***姿态角；
(3)雷达阵面开始旋转，在旋转过程中，利用捷联惯导数值递推姿态更新算法，进行***四元数和***方向余弦矩阵的迭代更新，同时更新过程量***姿态角；
并实时将***俯仰角与阈值45°比较。若***俯仰角小于45°，则处于直立状态，则将***四元数赋值输出四元数，然后利用四元数与姿态角间转换关系，计算输出姿态角；若***俯仰角大于等于45°，则处于倒伏状态，则转换惯导的载体坐标系，利用转换四元数将***四元数转换至输出四元数，然后利用四元数与姿态角间转换关系，计算输出姿态角初值，再将俯仰角单独进行数值输出连续性处理，进而得到输出姿态角终值。
本捷联惯导全姿态表示方法的主要发明点如下：
1、惯导对准完成进入纯惯性阶段，得到雷达阵面姿态角。其中，惯导内部姿态解算全时段采用四元数与方向余弦矩阵相结合的方式，只在惯导载体坐标系转换判定时和姿态角输出时使用欧拉角，以此避免欧拉角奇异性对于姿态更新精度的影响。记内部解算得到的姿态角过程量为***姿态角，最终对外输出的姿态角为输出姿态角。
2、雷达阵面运动过程中，不需要外部信息指示，根据***俯仰角与阈值间大小关系，实时自主判断是否进行坐标系转换。阵面由直立状态向倒伏状态旋转过程中，***俯仰角θ由0°左右逐步增大，在这个过程中实时判定***俯仰角θ范围：若θ小于45°，则处于直立状态，无需转换处理，输出姿态角以直立坐标系为基础，输出姿态角与***姿态角一致；若θ大于等于45°，则处于倒伏状态，利用转换四元数将直立坐标系依据右手定则绕x轴按x轴的负方向旋转90°得到倒伏坐标系，以倒伏坐标系作为当前惯导载体坐标系，输出姿态角以倒伏坐标系为基础重新计算。如上操作，惯导在直立状态和倒伏状态之间切换时，在不需要外部信息指示的情况下，可实时自主判定当前状态，自行决定采用可以规避欧拉角奇异性的合理的坐标系输出满足精度要求的姿态角。
3、对输出俯仰角做连续性处理。雷达阵面在阈值45°处时，直立坐标系下的输出俯仰角为45°；倒伏坐标系下的输出俯仰角为-45°。即惯导载体坐标系的旋转导致了90°的俯仰角跳跃。为了让姿态角输出具有连续性，在倒伏坐标系下的输出俯仰角的基础上，输出俯仰角执行加90°的处理。即在倒伏坐标系下，45°切换后的输出俯仰角仍为为45°，与直立坐标系输出值保持连续。如上操作，惯导可以在两种状态下随意切换，在[0°,90°]完整的角度工作区间内，惯导输出姿态角均保证了连续性和准确性。
4、倒伏状态下，转换四元数实现坐标系旋转。倒伏状态下的坐标系旋转是在直立坐标系(见图2)基础上，依据右手定则绕x轴按x轴的负方向旋转90°，获得倒伏坐标系，见图3。旋转后的在倒伏坐标系下的横滚角和方位角与直立坐标系下的一致，俯仰角相比直立坐标系则偏小90°，这样就避开了欧拉角奇异性产生的区间。下面计算转换四元数。记横滚角为γ，俯仰角为θ，方位角为Ψ，四元数为q，则姿态角与四元数转换关系：
Figure BDA0003263511350000051
直立坐标系下t时刻雷达阵面的姿态四元数为Qt，倒伏坐标系下t时刻雷达阵面的姿态四元数为qt。直立坐标系至倒伏坐标系的转换角度为俯仰角-90°，依据上面的姿态角与四元数转换关系得到对应该转换角度的转换四元数为：
Figure BDA0003263511350000052
那么存在下述关系：
qt＝Qt*q
其中，*代表四元数乘法运算；
5、姿态角输出。
记t时刻输出四元数为：
Figure BDA0003263511350000053
则在直立状态下，
Figure BDA0003263511350000056
在倒伏状态下，/>
Figure BDA0003263511350000057
接着，利用输出四元数提取输出姿态角：
θ＝arcsin(2(q2q3+q0q1))
Figure BDA0003263511350000054
Figure BDA0003263511350000055
尽管为说明目的公开了本发明的实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神范围内，各种替换、变化和修改都是可以的，因此，本发明的范围不局限于实施例和附图所公开的内容。

Claims (2)

1.一种针对雷达阵面旋转的捷联惯导全姿态表示方法，其特征在于：包括如下步骤：
步骤1、建立初始条件：惯导固定于雷达车的雷达阵面上，设定不存在安装误差，将雷达阵面转动至直立位，设定该位置为雷达阵面俯仰角真值0°位置附近；在惯导的控制***内预设有两个载体坐标系，一个为直立坐标系，另一个为倒伏坐标系；直立坐标系定义为：雷达阵面俯仰角真值0°位置时，坐标系原点位于惯导质心，x轴指向雷达车左侧，y轴指向雷达车尾，z轴指向天，倒伏坐标系的定义为：依据右手定则，实时由当前的直立坐标系绕x轴按x轴的负方向旋转90°获得；
步骤2、惯导上电后开始对准，雷达阵面维持几分钟不动，对准完成后，得到雷达阵面在俯仰角0°附近时惯导姿态对应的***四元数、***方向余弦矩阵、***姿态角；
步骤3、绕阵面旋转中心点转动雷达阵面，在转动的过程中利用捷联惯导数值递推姿态更新算法，进行***四元数和***方向余弦矩阵的迭代更新，同时更新过程量***姿态角；并在该过程中，实时将***俯仰角与阈值45°比较；若***俯仰角小于45°，则在直立坐标系下，将***四元数赋值输出四元数，然后利用四元数与姿态角间转换关系，计算得到输出姿态角，输出姿态角包括输出横滚角、输出方位角和输出俯仰角三个参量，若***俯仰角大于等于45°，则利用转换四元数将惯导载体坐标系由当前直立坐标系依据右手定则绕x轴按x轴的负方向旋转90°得到倒伏坐标系，在倒伏坐标系下计算得到输出姿态角。
2.根据权利要求1所述的针对雷达阵面旋转的捷联惯导全姿态表示方法，其特征在于：在任意时刻，倒伏坐标系下计算输出的横滚角和方位角均与在直立坐标系下的一致，俯仰角相比直立坐标系存在-90°的固定差；
输出姿态角的计算过程如下：
1）计算转换四元数：记横滚角为γ，俯仰角为θ，方位角为Ψ，四元数为q，则姿态角与四元数转换关系：
Figure QLYQS_1
直立坐标系下t时刻雷达阵面的姿态四元数为Qt，倒伏坐标系下t时刻雷达阵面的姿态四元数为qt；直立坐标系至倒伏坐标系的转换角度为俯仰角-90°，依据上面的姿态角与四元数转换关系得到对应该转换角度的转换四元数为：/>
Figure QLYQS_2
存在下述关系：qt=Qt*qtrans
其中，*代表四元数乘法运算；
2）计算输出姿态角
定义t时刻输出四元数为：
Figure QLYQS_3
则在直立状态下，/>
Figure QLYQS_4
；在倒伏状态下，
Figure QLYQS_5
；
利用输出四元数提取输出姿态角：
Figure QLYQS_6
。
CN202111079664.8A 2021-09-15 2021-09-15 一种针对雷达阵面旋转的捷联惯导全姿态表示方法 Active CN113686333B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111079664.8A CN113686333B (zh) 2021-09-15 2021-09-15 一种针对雷达阵面旋转的捷联惯导全姿态表示方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111079664.8A CN113686333B (zh) 2021-09-15 2021-09-15 一种针对雷达阵面旋转的捷联惯导全姿态表示方法

Publications (2)

Publication Number Publication Date
CN113686333A CN113686333A (zh) 2021-11-23
CN113686333B true CN113686333B (zh) 2023-06-20

Family

ID=78586384

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111079664.8A Active CN113686333B (zh) 2021-09-15 2021-09-15 一种针对雷达阵面旋转的捷联惯导全姿态表示方法

Country Status (1)

Country Link
CN (1) CN113686333B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117405109B (zh) * 2023-12-15 2024-04-16 北京神导科技股份有限公司 基于四元数球面线性加权的三套惯导***姿态表决方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101814194A (zh) * 2010-04-08 2010-08-25 合肥工业大学 一种消除欧拉角奇异性的转角增量方法
CN112284412A (zh) * 2020-09-09 2021-01-29 上海航天控制技术研究所 一种避免欧拉转换奇异导致精度下降的地面静态对准方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101814194A (zh) * 2010-04-08 2010-08-25 合肥工业大学 一种消除欧拉角奇异性的转角增量方法
CN112284412A (zh) * 2020-09-09 2021-01-29 上海航天控制技术研究所 一种避免欧拉转换奇异导致精度下降的地面静态对准方法

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
冯春 ; 吴洪涛 ; 陈柏 ; 乔兵 ; .追踪与目标航天器相对位姿参数四元数的解析算法.中国机械工程.2013,(第12期),全文. *
刘海颖 ; 王惠南 ; 刘新文 ; .基于UKF的四元数载体姿态确定.南京航空航天大学学报.2006,(第01期),全文. *
施佳良 ; 宋来亮 ; 江泽 ; .双目视觉与惯性测量单元相对姿态在线标定.无线互联科技.2019,(第24期),全文. *
王晓晨 ; 赵辉 ; 马克茂 ; 霍鑫 ; 姚郁 ; .一种新型全姿态飞行仿真转台的运动学分析.中国科学:信息科学.2010,(第04期),全文. *
翁海娜 ; 赵圆 ; 李天晖 ; .激光捷联***中的优化圆锥误差补偿算法.中国惯性技术学报.2008,(第02期),全文. *
路平 ; 肖文健 ; .倾转机身无人机俯仰角奇异问题研究.飞行力学.2013,(第05期),全文. *
郑荣才 ; 陈超英 ; 杨功流 ; .基于多IMU融合的甲板变形监测.机械强度.2007,(第04期),全文. *

Also Published As

Publication number Publication date
CN113686333A (zh) 2021-11-23

Similar Documents

Publication Publication Date Title
CN110927740B (zh) 一种移动机器人定位方法
CN110609290B (zh) 激光雷达匹配定位方法及装置
CN107687850B (zh) 一种基于视觉和惯性测量单元的无人飞行器位姿估计方法
CN109755995B (zh) 基于ros机器人操作***的机器人自动充电对接方法
CN110969145B (zh) 一种遥感图像匹配优化方法、装置、电子设备及存储介质
CN112764074B (zh) 一种定位导航轨迹的方法、装置及电子设备
CN113686333B (zh) 一种针对雷达阵面旋转的捷联惯导全姿态表示方法
CN111750865A (zh) 一种用于双功能深海无人潜器导航***的自适应滤波导航方法
CN114241050B (zh) 一种基于曼哈顿世界假设及因子图的相机位姿优化方法
CN115436920A (zh) 一种激光雷达标定方法及相关设备
CN113295171B (zh) 一种基于单目视觉的旋转刚体航天器姿态估计方法
CN112219225A (zh) 定位方法、***及可移动平台
CN114593735A (zh) 一种位姿预测方法及装置
WO2023241556A1 (zh) 泊车控制方法、装置、设备和存储介质
CN116579989B (zh) 一种基于深度相机的隧道打孔倾角修正方法
CN116124161B (zh) 一种基于先验地图的LiDAR/IMU融合定位方法
CN114565670A (zh) 一种位姿优化方法及装置
CN111340870B (zh) 基于视觉的拓扑地图生成方法
CN109919998B (zh) 卫星姿态确定方法、装置和终端设备
CN109737939B (zh) 一种实现电子罗盘全方位安装的方法及装置
EP4382954A2 (en) Lidar odometry implementation method, computer device, storage medium, and vehicle
CN114018271B (zh) 一种基于陆标图像的精确定点着陆自主导航方法和***
CN113672862B (zh) 一种测量向量分量缺失情况下欧拉角优化方法
CN113819918B (zh) 一种定位方法、装置、电子设备以及存储介质
CN118411421A (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
GR01 Patent grant
GR01 Patent grant