CN114166216B - 一种aru正置与倒置安装的水平姿态测量方法 - Google Patents
一种aru正置与倒置安装的水平姿态测量方法 Download PDFInfo
- Publication number
- CN114166216B CN114166216B CN202111410041.4A CN202111410041A CN114166216B CN 114166216 B CN114166216 B CN 114166216B CN 202111410041 A CN202111410041 A CN 202111410041A CN 114166216 B CN114166216 B CN 114166216B
- Authority
- CN
- China
- Prior art keywords
- angle
- aru
- horizontal attitude
- gamma
- attitude
- 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
Links
- 238000009434 installation Methods 0.000 title claims abstract description 25
- 238000000691 measurement method Methods 0.000 title abstract description 7
- 238000000034 method Methods 0.000 claims abstract description 22
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 238000005259 measurement Methods 0.000 claims abstract description 11
- 238000012937 correction Methods 0.000 claims abstract description 9
- 238000012545 processing Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000004422 calculation algorithm Methods 0.000 claims description 9
- 230000036544 posture Effects 0.000 description 12
- 238000004088 simulation Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种ARU正置与倒置安装的水平姿态测量方法,包括以下步骤:1.对ARU姿态参考单元***进行初始对准;2.以水平失准角和陀螺漂移作状态量初始化卡尔曼滤波器;3.递推k时刻的姿态四元数Qk;4.计算修正前的k时刻的水平姿态角;5.卡尔曼滤波时间更新;6.选取加速度计输出fp做量测量进行量测更新,利用更新后的状态量对修正前的k时刻的水平姿态角进行修正,并对其进行处理获得正确的水平姿态角。重复进行步骤3‑6就完成了ARU正置与倒置安装的水平姿态的测量。本发明解决了ARU正置与倒置安装情况下的水平姿态测量的问题。
Description
技术领域
本发明属于姿态测量技术领域,涉及一种ARU的水平姿态测量方法,特别是一种ARU正置与倒置安装的水平姿态测量方法。
背景技术
传统的惯性导航***可以给出载体的位置、速度和姿态等信息,随着越来越多样化需求的产生,在某些场合,例如水面舰船以及陆地汽车,我们仅需要对载体或部件的水平姿态进行实时测量。ARU(姿态参考单元)就是一种基于捷联惯导技术的专门用于测量载体的水平姿态(纵摇角、横摇角)的设备。
目前,水平姿态测量的方法越来越多,例如在专利申请号为201911203231.1,名称为“基于运动参考单元的水面舰船水平姿态测量方法”的专利文件中,通过利用加速度计和陀螺仪输出进行运载体运动状态判断,根据不同的运动状态采用不同的方法计算失准角,然后利用失准角实时修正计算四元数得到准确的水平姿态。又如在专利申请号为202110428645.5,名称为“一种基于运动状态监测的自适应水平姿态测量方法”的专利文件中,通过建立新的状态空间模型,将水平姿态角作为状态变量,补偿随机常值零偏的角速度增量为***方程的控制输入,补偿随机常值零偏的比力作为量测量,同时对载体机动状态的判断条件进行改善,综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息进行判断判断,实现对滤波器量测噪声阵的自动调整,有效降低了载体机动对水平姿态解算的影响。
目前的水平姿态测量方法大多是针对设备正向放置安装进行设计的,即设备内部的加速度计是沿载体坐标系安装的,并没有明确指出是否可以适用于设备倒置安装的情况。而在实际工程应用中,例如一些小型的AUV等,预留给设备的安装空间是有限的,其出线端(如数据电源线等)也是固定的,这就会导致设备有时并不能正向放置安装,而是倒置安装。当ARU倒置安装的时候,若继续采用ARU正向放置安装的程序,此时得到的水平姿态与载体的实际水平姿态是不符的。如果采用与倒置安装符合的程序,此时就可以得到与载体实际水平姿态相符合的姿态值。但是在实际应用中采用两套程序,会增加该设备的使用成本,因此研究一种能够在ARU正置与倒置安装情况下都可以测量水平姿态的方法是具有实际工程价值的。
发明内容
针对上述现有技术,本发明要解决的技术问题是提供一种能够在ARU正置与倒置安装情况下都可以测量水平姿态的ARU正置与倒置安装的水平姿态测量方法。
为解决上述技术问题,本发明的一种ARU正置与倒置安装的水平姿态测量方法,包括:
步骤1:对ARU姿态参考单元***进行初始对准得到初始水平姿态角,包括纵摇角θ0和横摇角γ0,并求解获得初始姿态四元数Q0和初始捷联矩阵T0;
步骤2:初始化卡尔曼滤波器,令X0=[φ0 ε0]T,其中φ0为初始水平失准角,φ0=[φE0,φN0],φE0为东向初始失准角,φN0为北向初始失准角,ε0为初始陀螺漂移,初始均方误差为P0,初始化k=1;
步骤3:利用k-1时刻的姿态四元数Qk-1递推k时刻的姿态四元数Qk;
步骤4:利用步骤3得到的Qk求解k时刻的捷联矩阵Tk,并利用Tk反解求出修正前的k时刻的水平姿态角,包括纵摇角θk与横摇角γk;
步骤5:进行卡尔曼滤波时间更新;
步骤6:选取加速度计输出的转换到p坐标系下的fp作为卡尔曼滤波量测量进行量测更新,实现卡尔曼滤波状态量的校正,利用校正后的卡尔曼滤波状态量对k时刻的捷联矩阵Tk进行修正从而得到修正后的k时刻的捷联矩阵T′k,利用T′k解出修正后的k时刻的水平姿态角,包括纵摇角θ′k、横摇角γ′k,再对θ′k与γ′k进行处理获得正确的水平姿态角,包括纵摇角θout、横摇角γout,利用正确的水平姿态角解出修正后的姿态四元数Q′k;
步骤7:令k=k+1,步骤3中k-1时刻的姿态四元数Qk-1等于步骤6中得到的Q′k,重复进行步骤3-7至导航工作状态结束。
进一步的,步骤6中对θ′k与γ′k进行处理获得正确的水平姿态角,包括纵摇角θout、横摇角γout具体为:
θout=θ′k
其中,代表算法判断此时ARU处于倒置安装,γ′k∈(-90°,+90°)代表算法判断此时ARU处于正置安装,式中,θ′k与γ′k的表达式为:
其中,T′ij为修正后的k时刻的捷联矩阵T′k的第i行第j列元素(i,j=1,2,3)。asin(·)代表·的反正弦值,atan2(·)代表·的反正切值。
本发明的有益效果:本发明提供了一种ARU正置与倒置安装的水平姿态测量方法,针对需要ARU设备倒置的情况对算法进行了改进,使得该算法在ARU设备正置和倒置安装情况下都可得到正确的水平姿态信息,有一定的工程应用价值。
附图说明
图1为本发明设备正置和倒置坐标系示意图;
图2为本发明的ARU正置与倒置安装的水平姿态测量方法流程图。
具体实施方式
下面结合说明书附图和具体实施例对本发明做进一步说明。
结合图2,本发明包括以下步骤:
步骤1.对ARU姿态参考单元***进行初始对准得到初始水平姿态角(纵摇角θ0、横摇角γ0)并求解获得初始姿态四元数Q0和初始捷联矩阵T0。
步骤2.初始化卡尔曼滤波器,设置初始水平失准角φE0、φN0和陀螺漂移ε0作为卡尔曼滤波状态量X0=[φ0 ε0]T的初始值,初始均方误差为P0。
步骤3.递推k时刻的姿态四元数Qk。即利用k-1时刻的姿态四元数Qk-1和k-1时刻到k时刻的角增量得到k时刻的姿态四元数Qk为:
其中Δηk-1为的模值;/>代表四元数乘法,cos(·)代表·的余弦值,sin(·)代表·的正弦值。
步骤4.计算修正前的k时刻的水平姿态角(纵摇角θk、横摇角γk)。利用步骤3中获得k时刻的姿态四元数Qk求解k时刻的捷联矩阵Tk,并利用Tk反解求出修正前的k时刻的水平姿态角(纵摇角θk、横摇角γk)。
步骤5.进行卡尔曼滤波时间更新。其中相关方程的建立:
其中为k-1时刻状态最优估计,Pk-1为k-1时刻状态估计的均方误差阵,/>为k时刻状态一步预测,/>为k时刻状态一步转移矩阵,其中T1是一行三列的行向量包含的三个元素为捷联矩阵T的第一行的三个元素,T2是一行三列的行向量包含的三个元素为捷联矩阵T的第二行的三个元素,03×1是三行一列的行向量且元素全为0,β=diag(1/τ11/τ21/τ3),其中τi(i=1,2,3)为陀螺仪一阶马尔科夫过程随机误差相应的相关时间常数,Pk/k-1为k时刻状态一步预测均方误差阵,Nk-1为k-1时刻***噪声方差阵。
步骤6.将加速度计输出fb由载体坐标系b系转换到平台坐标系p系得到fp。即fp=T·fb,其中T为捷联矩阵。
步骤7.选取步骤6中的转换到p系的加速度计输出fp做卡尔曼滤波量测量进行量测更新,实现卡尔曼滤波状态量的校正。利用校正后的卡尔曼滤波状态量对k时刻的捷联矩阵Tk进行修正从而得到修正后的k时刻的捷联矩阵T′k。利用T′k解出修正后的k时刻的水平姿态角(纵摇角θ′k、横摇角γ′k),再对修正后的k时刻的水平姿态角进行处理获得正确的水平姿态角(纵摇角θout、横摇角γout)。利用正确的水平姿态角解出修正后的姿态四元数Q′k。其中,具体的量测更新方程为:
其中为k时刻量测矩阵,g为重力加速度,Rk为k时刻量测噪声方差阵,Kk为k时刻滤波增益,/>为k时刻状态估计,Zk为k时刻量测量,Pk为k时刻状态估计均方误差阵,I为单位矩阵。
对修正后的k时刻的水平姿态角(纵摇角θk′、横摇角γ′k)进行处理获得正确的水平姿态角(纵摇角θout、横摇角γout),即对反三角函数求出的水平姿态角主值θk′与γ′k进行判断与处理,从而得到水平姿态角真值θout与γout,其具体表达式为:
θout=θ′k (5)
其中,θk′与γ′k为修正后的k时刻的水平姿态角(水平姿态角主值),θout与γout为正确的水平姿态角(水平姿态角真值),代表算法判断此时ARU处于倒置安装,γ′k∈(-90°,+90°)代表算法判断此时ARU处于正置安装。式中,θ′k与γ′k的表达式为:
其中,T′ij为修正后的k时刻的捷联矩阵T′k的第i行第j列元素(i,j=1,2,3);asin(·)代表·的反正弦值,atan2(·)代表·的反正切值。
步骤8.利用步骤7中求解的修正后的姿态四元数Qk′作为步骤3中k-1时刻的姿态四元数Qk-1,重复进行步骤3-8至导航工作状态结束,至此,就完成了ARU正置和倒置安装情况下的水平姿态测量。
为了验证所述方法的有益效果,针对ARU正置情况,本发明利用转台实验数据进行了仿真实验。通过与转台解码的水平姿态信息做差值得到水平姿态误差范围,其结果如表1所示;针对ARU倒置安装情况,本发明将ARU倒置转变为与其效果相同的部分程序添加在方法中进行仿真实验。在给定舰船水平姿态的情况下,观察ARU设备输出的水平姿态角;仿真结果如表2所示,在ARU设备倒置时其水平姿态经过处理后得到的ARU水平姿态输出与舰船实际水平姿态相等。
表1算法误差
表2 ARU设备倒置时其水平姿态输出与舰船实际水平姿态对比
Claims (2)
1.一种ARU正置与倒置安装的水平姿态测量方法,其特征在于,包括:
步骤1:对ARU姿态参考单元***进行初始对准得到初始水平姿态角,包括纵摇角θ0和横摇角γ0,并求解获得初始姿态四元数Q0和初始捷联矩阵T0;
步骤2:初始化卡尔曼滤波器,令X0=[φ0 ε0]T,其中φ0为初始水平失准角,φ0=[φE0,φN0],φE0为东向初始失准角,φN0为北向初始失准角,ε0为初始陀螺漂移,初始均方误差为P0,初始化k=1;
步骤3:利用k-1时刻的姿态四元数Qk-1递推k时刻的姿态四元数Qk;
步骤4:利用步骤3得到的Qk求解k时刻的捷联矩阵Tk,并利用Tk反解求出修正前的k时刻的水平姿态角,包括纵摇角θk与横摇角γk;
步骤5:进行卡尔曼滤波时间更新;
步骤6:选取加速度计输出的转换到p坐标系下的fp作为卡尔曼滤波量测量进行量测更新,实现卡尔曼滤波状态量的校正,利用校正后的卡尔曼滤波状态量对k时刻的捷联矩阵Tk进行修正从而得到修正后的k时刻的捷联矩阵T′k,利用T′k解出修正后的k时刻的水平姿态角,包括纵摇角θ′k、横摇角γ′k,再对θ′k与γ′k进行处理获得正确的水平姿态角,包括纵摇角θout、横摇角γout,利用正确的水平姿态角解出修正后的姿态四元数Q′k;
步骤7:令k=k+1,步骤3中k-1时刻的姿态四元数Qk-1等于步骤6中得到的Q′k,重复进行步骤3-7至导航工作状态结束。
2.根据权利要求1所述的一种ARU正置与倒置安装的水平姿态测量方法,其特征在于:步骤6所述对θ′k与γ′k进行处理获得正确的水平姿态角,包括纵摇角θout、横摇角γout具体为:
θout=θ′k
其中,代表算法判断此时ARU处于倒置安装,γ′k∈(-90°,+90°)代表算法判断此时ARU处于正置安装,式中,θ′k与γ′k的表达式为:
其中,T′ij为修正后的k时刻的捷联矩阵T′k的第i行第j列元素(i,j=1,2,3),asin(·)代表·的反正弦值,atan2(·)代表·的反正切值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111410041.4A CN114166216B (zh) | 2021-11-25 | 2021-11-25 | 一种aru正置与倒置安装的水平姿态测量方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111410041.4A CN114166216B (zh) | 2021-11-25 | 2021-11-25 | 一种aru正置与倒置安装的水平姿态测量方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114166216A CN114166216A (zh) | 2022-03-11 |
CN114166216B true CN114166216B (zh) | 2023-07-21 |
Family
ID=80481017
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111410041.4A Active CN114166216B (zh) | 2021-11-25 | 2021-11-25 | 一种aru正置与倒置安装的水平姿态测量方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114166216B (zh) |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107478223A (zh) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 |
CN112629538A (zh) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 |
CN113155129A (zh) * | 2021-04-02 | 2021-07-23 | 北京大学 | 一种基于扩展卡尔曼滤波的云台姿态估计方法 |
WO2021227012A1 (zh) * | 2020-05-11 | 2021-11-18 | 中国科学院地质与地球物理研究所 | 一种姿态测量方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160047675A1 (en) * | 2005-04-19 | 2016-02-18 | Tanenhaus & Associates, Inc. | Inertial Measurement and Navigation System And Method Having Low Drift MEMS Gyroscopes And Accelerometers Operable In GPS Denied Environments |
-
2021
- 2021-11-25 CN CN202111410041.4A patent/CN114166216B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107478223A (zh) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 |
WO2021227012A1 (zh) * | 2020-05-11 | 2021-11-18 | 中国科学院地质与地球物理研究所 | 一种姿态测量方法 |
CN112629538A (zh) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 |
CN113155129A (zh) * | 2021-04-02 | 2021-07-23 | 北京大学 | 一种基于扩展卡尔曼滤波的云台姿态估计方法 |
Non-Patent Citations (1)
Title |
---|
自适应CDPF及其在组合导航中的应用;薛丽;高社生;杨一;;计算机仿真(第01期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114166216A (zh) | 2022-03-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN100516775C (zh) | 一种捷联惯性导航***初始姿态确定方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN106871928B (zh) | 基于李群滤波的捷联惯性导航初始对准方法 | |
CN107270938B (zh) | 基于泰勒级数拟合的单轴旋转惯导***姿态解调方法 | |
CN105806363B (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
CN109931955B (zh) | 基于状态相关李群滤波的捷联惯性导航***初始对准方法 | |
CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
CN112798021B (zh) | 基于激光多普勒测速仪的惯导***行进间初始对准方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN109612460B (zh) | 一种基于静止修正的垂线偏差测量方法 | |
CN110954102A (zh) | 用于机器人定位的磁力计辅助惯性导航***及方法 | |
CN111121766A (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
CN102538788B (zh) | 一种基于状态估计和预测的低成本阻尼导航方法 | |
CN111795708B (zh) | 晃动基座条件下陆用惯性导航***的自适应初始对准方法 | |
CN114777812B (zh) | 一种水下组合导航***行进间对准与姿态估计方法 | |
CN114964222A (zh) | 一种车载imu姿态初始化方法、安装角估计方法及装置 | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN112212862A (zh) | 一种改进粒子滤波的gps/ins组合导航方法 | |
CN112798016A (zh) | 一种基于sins与dvl组合的auv行进间快速初始对准方法 | |
CN113137977B (zh) | 一种sins/偏振光组合导航初始对准滤波方法 | |
CN104634348B (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 |