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

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

Info

Publication number
CN113686333A
CN113686333A CN202111079664.8A CN202111079664A CN113686333A CN 113686333 A CN113686333 A CN 113686333A CN 202111079664 A CN202111079664 A CN 202111079664A CN 113686333 A CN113686333 A CN 113686333A
Authority
CN
China
Prior art keywords
angle
coordinate system
attitude
array surface
quaternion
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.)
Granted
Application number
CN202111079664.8A
Other languages
English (en)
Other versions
CN113686333B (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)
  • Radar Systems Or Details Thereof (AREA)
  • Navigation (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 FDA0003263511340000011
直立坐标系下t时刻雷达阵面的姿态四元数为Qt，倒伏坐标系下t时刻雷达阵面的姿态四元数为qt。直立坐标系至倒伏坐标系的转换角度为俯仰角-90°，依据上面的姿态角与四元数转换关系得到对应该转换角度的转换四元数为：
Figure FDA0003263511340000021
存在下述关系：
qt＝Qt*q
其中，*代表四元数乘法运算；
2)计算输出姿态角
定义t时刻输出四元数为：
Figure FDA0003263511340000022
则在直立状态下，
Figure FDA0003263511340000023
在倒伏状态下，
Figure FDA0003263511340000024
利用输出四元数提取输出姿态角：
θ＝arcsin(2(q2q3+q0q1))
Figure FDA0003263511340000025
Figure FDA0003263511340000026
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 true CN113686333A (zh) 2021-11-23
CN113686333B 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)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117405109A (zh) * 2023-12-15 2024-01-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
冯春;吴洪涛;陈柏;乔兵;: "追踪与目标航天器相对位姿参数四元数的解析算法" *
刘海颖;王惠南;刘新文;: "基于UKF的四元数载体姿态确定" *
施佳良;宋来亮;江泽;: "双目视觉与惯性测量单元相对姿态在线标定" *
王晓晨;赵辉;马克茂;霍鑫;姚郁;: "一种新型全姿态飞行仿真转台的运动学分析" *
翁海娜;赵圆;李天晖;: "激光捷联系统中的优化圆锥误差补偿算法" *
路平;肖文健;: "倾转机身无人机俯仰角奇异问题研究" *
郑荣才;陈超英;杨功流;: "基于多IMU融合的甲板变形监测" *

Cited By (2)

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

Also Published As

Publication number Publication date
CN113686333B (zh) 2023-06-20

Similar Documents

Publication Publication Date Title
CN107687850B (zh) 一种基于视觉和惯性测量单元的无人飞行器位姿估计方法
CN110609290B (zh) 激光雷达匹配定位方法及装置
CN109755995B (zh) 基于ros机器人操作系统的机器人自动充电对接方法
CN109974727B (zh) 一种机器人充电方法、装置及机器人
CN111578957B (zh) 基于三维点云地图定位的智能车纯追踪循迹方法
CN106950953B (zh) 一种自动泊车路径跟踪及车辆控制系统和方法
CN110926485B (zh) 一种基于直线特征的移动机器人定位方法及系统
JP2021140822A (ja) 車両制御方法、車両制御装置及び車両
CN108398701B (zh) 车辆定位方法和装置
CN112764074B (zh) 一种定位导航轨迹的方法、装置及电子设备
CN113706589A (zh) 车载激光雷达点云配准方法、装置、电子设备及存储介质
EP4345421A2 (en) Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle
CN113686333A (zh) 一种针对雷达阵面旋转的捷联惯导全姿态表示方法
CN114136315A (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN114241050B (zh) 一种基于曼哈顿世界假设及因子图的相机位姿优化方法
CN115326051A (zh) 一种基于动态场景的定位方法、装置、机器人及介质
CN112219225A (zh) 定位方法、系统及可移动平台
CN111123914A (zh) 割草机器人基于视觉场景的方向估算方法
WO2023241556A1 (zh) 泊车控制方法、装置、设备和存储介质
CN111121779B (zh) 一种无人机所处飞行区域的实时检测方法
CN116576877A (zh) 一种户外无人清洁车的定位和路径跟踪方法
WO2023035359A1 (zh) 一种组合循迹方法及装置
CN115214693B (zh) 横摆角速度修正方法、装置以及车辆
CN113503883B (zh) 采集用于构建地图的数据的方法、存储介质及电子设备
CN115388878A (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