CN107806874A - 一种视觉辅助的捷联惯导极区初始对准方法 - Google Patents

一种视觉辅助的捷联惯导极区初始对准方法 Download PDF

Info

Publication number
CN107806874A
CN107806874A CN201710989714.3A CN201710989714A CN107806874A CN 107806874 A CN107806874 A CN 107806874A CN 201710989714 A CN201710989714 A CN 201710989714A CN 107806874 A CN107806874 A CN 107806874A
Authority
CN
China
Prior art keywords
msub
mrow
msubsup
coordinate system
mtd
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
CN201710989714.3A
Other languages
English (en)
Other versions
CN107806874B (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.)
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 CN201710989714.3A priority Critical patent/CN107806874B/zh
Publication of CN107806874A publication Critical patent/CN107806874A/zh
Application granted granted Critical
Publication of CN107806874B publication Critical patent/CN107806874B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Abstract

本发明提出一种视觉辅助的捷联惯导极区初始对准方法，包括视觉辅助解算、捷联惯导极区解算、组合导航滤波、反馈校正输出四个步骤；通过视觉辅助解算和捷联惯导极区解算分别获得载体位置、姿态角，以视觉辅助解算和捷联惯导极区解算获得的载体位置、姿态角的差值为量测值，利用卡尔曼滤波方法，估计出捷联惯导的姿态误差、速度误差和位置误差，进行组合导航滤波，估计出捷联惯导姿态失准角速度误差位置误差最后进行修正，得到更为精确的载体姿态角，速度和位置信息。本发明实现了捷联惯导在极区的初始对准，由于其不依靠其他高精度的导航设备(比如说不需要高精度的主惯导)，所以可以应用在更多的场合下。

Description

一种视觉辅助的捷联惯导极区初始对准方法
技术领域
本发明涉及捷联惯导的极区初始对准技术，涉及一种捷联惯导/视觉组合导航滤波方法，实现极区初始对准。
背景技术
由于地球自转角速度矢量和重力加速度矢量之间的夹角随纬度的增加而减小，直至重合，所以在极区捷联惯导自对准实现困难，须借助其他方法完成初始对准。
借助主惯导系统的传递对准是目前捷联惯导在极区进行初始对准的主要方法。在传递对准过程中，主惯导必须处于导航状态，将其速度输出看作无误差，并且主惯导的精度应比待对准的捷联惯导高，通过速度匹配和卡尔曼滤波技术，以主惯导与待对准的捷联惯导的速度差为量测值估计出失准角，估计出待对准的捷联惯导姿态误差角。
传递对准方法虽然可以实现捷联惯导在极区的初始对准，但是必须依靠高精度的主惯导才能完成，并且主惯导必须处于导航状态，所以其应用有很大的限制条件。
发明内容
为解决现有技术存在的问题，本发明提出一种视觉辅助的捷联惯导极区初始对准方法，将捷联惯导与视觉测量相结合，通过与捷联惯导固连的视觉测量系统测量位置、姿态已知的固定在地面上的标志物，实现了捷联惯导在极区的初始对准，由于其不依靠其他高精度的导航设备(比如说不需要高精度的主惯导)，所以可以应用在更多的场合下。
本发明的技术方案为：
所述一种视觉辅助的捷联惯导极区初始对准方法，其特征在于：包括以下步骤：
步骤1：进行视觉辅助导航解算：
步骤1.1：采用视觉传感器获取t时刻的图像，并从图像中提取至少6个特征点，将特征点信息代入以下公式
采用最小二乘解算得到t时刻导航坐标系nt到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置
其中，为已知的第i个特征点在导航坐标系下的坐标，为视觉传感器光学中心在导航坐标系下的位置坐标，[uiυi]T为从图像信息中获取的第i个特征点的成像在图像像素坐标系下的位置坐标，为第i个特征点到视觉传感器光学中心的距离在光轴上的投影距离，f/dx、f/dy为视觉传感器的等效焦距，u0、v0为视觉传感器光轴与图像的交点在图像像素坐标系下的位置坐标；所述导航坐标系采用横向地理坐标系gt；
步骤1.2：根据步骤1.1计算得到的导航坐标系nt到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置通过公式
以及
计算得到载体坐标系b到导航坐标系nt的转换矩阵和载体在导航坐标系下的位置坐标其中载体坐标系b到视觉传感器坐标系c的转换矩阵和平移矢量在视觉传感器进行标定后均已知；
步骤2：进行极区捷联惯导解算：
步骤2.1：建立载体的姿态，速度，位置微分方程为：
姿态微分方程：
其中表示的叉乘反对称矩阵，为横向地理坐标系gt相对于载体坐标系b的角速度在载体坐标系b下的投影；为载体坐标系b到横向地理坐标系gt的转换矩阵；
速度微分方程：
其中为载体速度在横向地理坐标系gt下的投影；为载体加速度在横向地理坐标系gt下的投影；为惯性坐标系相对于地球坐标系e的角速度在横向地理坐标系gt下的投影；为地球坐标系e相对于横向地理坐标系gt的角速度在横向地理坐标系gt下的投影；为重力加速度在横向地理坐标系gt下的投影；
位置微分方程：
位置方向余弦矩阵的微分方程为：
其中为横向地理坐标系gt到横向地球坐标系et的转换矩阵，为的叉乘反对称矩阵，为横向地球坐标系et相对于横向地理坐标系gt的角速度在横向地理坐标系gt下的投影；
载体横向纬度Lt、横向经度λt和载体高度h的微分方程为：
其中为载体在横向地理坐标系gt下的北向速度，Rot为地球的曲率半径，为载体在横向地理坐标系gt下的东向速度，为载体在横向地理坐标系gt下的天向速度；
步骤2.2：由姿态，速度，位置微分方程，按照捷联惯导导航解算方法，和为输入量，得到载体的姿态信息速度信息和位置信息，所述位置信息包括横向纬度Lt、横向经度λt；
步骤3：以步骤1结合步骤2得到的载体位置、姿态角的差值为量测值，利用卡尔曼滤波方法，估计出捷联惯导的姿态误差、速度误差和位置误差，进行组合导航滤波：
步骤3.1：滤波器的系统状态变量为：
其中为捷联惯导失准角，为速度误差，为位置误差，为陀螺常值漂移误差，为加速度计常值零偏误差，θ为捷联惯导与视觉辅助系统的失准角，为捷联惯导与视觉辅助系统的位置误差；
系统状态方程为：
其中nt为导航坐标系，导航坐标系nt采用横向地理坐标系gt；为惯性坐标系相对于导航坐标系nt的角速度在导航坐标系nt下的投影；为惯性坐标系相对于导航坐标系nt的角速度误差在导航坐标系nt下的投影；为惯性坐标系相对于载体坐标系b的角速度误差在载体坐标系b下的投影；为载体坐标系b到导航坐标系nt的姿态转换矩阵；fb为载体加速度在载体坐标系b下的投影；δfb为载体加速度误差在载体坐标系b下的投影，为惯性坐标系相对于横向地球坐标系et的角速度在导航坐标系nt下的投影；为横向地球坐标系et相对于导航坐标系nt的角速度在导航坐标系nt下的投影；
系统的量测方程为：
其中Mat2 Ang{}表示姿态转换矩阵对应的姿态角，ωc为视觉姿态位置测量误差和高阶误差项引起的量测噪声；为载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影，为载体坐标系b到视觉传感器坐标系c的转换矩阵，载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；
步骤3.2：根据系统的状态方程和观测方程，利用卡尔曼滤波方法，估计出捷联惯导姿态失准角速度误差位置误差
步骤4：根据步骤3估计得到的捷联惯导姿态失准角速度误差位置误差分别对捷联惯导的输出进行修正，得到更为精确的载体姿态角，速度和位置信息。
有益效果
本发明通过对计算机视觉系统的引入，提出了一种视觉辅助的捷联惯导极区初始对准方法，解决了捷联惯导无法在极区自对准的问题，并且不依靠其他高精度导航设备，应用场合更加广泛。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1为本发明视觉辅助的捷联惯导极区初始对准方法流程图。
图2为横向地球坐标系。
图3为图像的三种坐标系。
图4为视觉辅助系统原理说明。
具体实施方式
下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
首先结合附图3对视觉系统上定义三个坐标系进行说明：
图像像素坐标系：
图像像素坐标系是以图像平面左上角像元为原点，u轴和v轴分别对应成像平面的行数和列数，(u,v)表示以像素为单位的图像像素坐标。
图像平面坐标系：
图像平面与光轴垂直，(xp,yp)表示以毫米为单位的坐标。在xp，yp坐标系中，原点Op定义在图像平面与相机光轴的交点，并位于图像中心。
视觉传感器坐标系：
视觉传感器坐标系的原点定义在视觉传感器的光心Oc处，OcYc与光轴重合指向前方，OcZc轴与图像平面坐标系中的OpYp平行，OcXc与图像平面坐标系中的OpXp轴平行。
下面结合附图4对本发明原理进行说明。
本发明包含四个步骤，也可以认为是由视觉辅助模块、捷联惯导极区解算模块、组合导航滤波模块、反馈校正输出模块四部分组成。
步骤1：进行视觉辅助导航解算：
步骤1.1：采用视觉传感器获取t时刻的图像，并从图像中提取至少6个特征点，将特征点信息代入以下公式
采用最小二乘解算得到t时刻导航坐标系nt到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置所述导航坐标系采用横向地理坐标系gt；
其中，为已知的第i个特征点在导航坐标系下的坐标，为视觉传感器光学中心在导航坐标系下的位置坐标，[ui υi]T为从图像信息中获取的第i个特征点的成像在图像像素坐标系下的位置坐标，为第i个特征点到视觉传感器光学中心的距离在光轴上的投影距离，f/dx、f/dy为视觉传感器的等效焦距，u0、v0为视觉传感器光轴与图像的交点在图像像素坐标系下的位置坐标；对于已经标定过的视觉传感器，f、dx、dy、u0、v0为已知量。
步骤1.2：根据步骤1.1计算得到的导航坐标系nt到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置通过公式
以及
计算得到载体坐标系b到导航坐标系nt的转换矩阵和载体在导航坐标系下的位置坐标由于视觉传感器和载体固连，所以视觉传感器标定后，载体坐标系b到视觉传感器坐标系c的转换矩阵和平移矢量在视觉传感器进行标定后均已知。
步骤2：进行极区捷联惯导解算：
以90°E/W经线圈作为伪赤道，以0°/180°经线圈作为伪本初子午圈，赤道与伪本初子午圈的交点为伪极点，这就被称为伪地球坐标系或者称为横向地球坐标系。在横向地球坐标系下进行导航解算，克服了真实地球极点处经线收敛带来的定向和定位困难。
由横向地球坐标系的定义，地球坐标系e绕oye转动-90°即可获得横向地球坐标系et，根据基本旋转理论可得地球坐标系e到横向地球坐标系et的转换关系为：
载体在地理坐标系和横向地理坐标系下的位置转换关系为：
Lt＝-arcsin(cosLcosλ)
L＝arcsin(cosLtcosλt)
Lt为载体在横向地理坐标系下的纬度，λt为载体在横向地理坐标系下的经度，L为载体在地理坐标系下的纬度，λ为载体在地理坐标系下的经度
地理坐标系g到横向地理坐标系gt的转换关系为：
为横向地球坐标系et到横向地理坐标系gt的转换矩阵，为地理坐标系g到地球坐标系e的转换矩阵；
其中：
横向惯性导航将横向地理坐标系gt作为导航坐标系nt进行解算，其力学编排与指北方位惯导系统相似。
步骤2.1：建立载体的姿态，速度，位置微分方程为：
姿态微分方程：
其中表示的叉乘反对称矩阵，为横向地理坐标系gt相对于载体坐标系b的角速度在载体坐标系b下的投影；为载体坐标系b到横向地理坐标系gt的转换矩阵；
速度微分方程：
其中为载体速度在横向地理坐标系gt下的投影；为载体加速度在横向地理坐标系gt下的投影；为惯性坐标系相对于地球坐标系e的角速度在横向地理坐标系gt下的投影；为地球坐标系e相对于横向地理坐标系gt的角速度在横向地理坐标系gt下的投影；为重力加速度在横向地理坐标系gt下的投影；
位置微分方程：
在惯性导航系统中将横向地理坐标系作为导航坐标系，位置方向余弦矩阵的微分方程为：
其中为横向地理坐标系gt到横向地球坐标系et的转换矩阵，为的叉乘反对称矩阵，为横向地球坐标系et相对于横向地理坐标系gt的角速度在横向地理坐标系gt下的投影；
载体横向纬度Lt、横向经度λt和载体高度h的微分方程为：
其中为载体在横向地理坐标系gt下的北向速度，Rot为地球的曲率半径，为载体在横向地理坐标系gt下的东向速度，为载体在横向地理坐标系gt下的天向速度；
步骤2.2：由姿态，速度，位置微分方程，按照捷联惯导导航解算方法，和为输入量，得到载体的姿态信息速度信息和位置信息，所述位置信息包括横向纬度Lt、横向经度λt。
步骤3：以步骤1结合步骤2得到的载体位置、姿态角的差值为量测值，利用卡尔曼滤波方法，估计出捷联惯导的姿态误差、速度误差和位置误差，进行组合导航滤波：
步骤3.1：滤波器的系统状态变量为：
其中为捷联惯导失准角，为速度误差，为位置误差，为陀螺常值漂移误差，为加速度计常值零偏误差，θ为捷联惯导与视觉辅助系统的失准角，为捷联惯导与视觉辅助系统的位置误差；
系统状态方程为：
其中nt为导航坐标系，导航坐标系nt采用横向地理坐标系gt；为惯性坐标系相对于导航坐标系nt的角速度在导航坐标系nt下的投影；为惯性坐标系相对于导航坐标系nt的角速度误差在导航坐标系nt下的投影；为惯性坐标系相对于载体坐标系b的角速度误差在载体坐标系b下的投影；为载体坐标系b到导航坐标系nt的姿态转换矩阵；fb为载体加速度在载体坐标系b下的投影；δfb为载体加速度误差在载体坐标系b下的投影，为惯性坐标系相对于横向地球坐标系et的角速度在导航坐标系nt下的投影；为横向地球坐标系et相对于导航坐标系nt的角速度在导航坐标系nt下的投影；
系统的量测方程为：
其中Mat2Ang{}表示姿态转换矩阵对应的姿态角，ωc为视觉姿态位置测量误差和高阶误差项引起的量测噪声；为载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影，为载体坐标系b到视觉传感器坐标系c的转换矩阵，载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；
在上述的组合模式中，其量测值z有两组：一组为位置量测差值，另一组为姿态量测差值；所谓位置量测差值是由捷联惯导系统给出的经度、纬度、高度信息和视觉辅助系统给出的相应的信息求差，作为一组量测值；所谓姿态量测差值是由捷联惯导系统给出的姿态信息和视觉辅助系统给出的姿态信息求差，作为一组量测值；
步骤3.2：根据系统的状态方程和观测方程，利用卡尔曼滤波方法，估计出捷联惯导姿态失准角速度误差位置误差
步骤4：根据步骤3估计得到的捷联惯导姿态失准角速度误差位置误差分别对捷联惯导的输出进行修正，得到更为精确的载体姿态角，速度和位置信息。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

1.一种视觉辅助的捷联惯导极区初始对准方法，其特征在于：包括以下步骤：
步骤1：进行视觉辅助导航解算：
步骤1.1：采用视觉传感器获取t时刻的图像，并从图像中提取至少6个特征点，将特征点信息代入以下公式
<mrow> <msubsup> <mi>Z</mi> <mi>i</mi> <mi>c</mi> </msubsup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mi>i</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;upsi;</mi> <mi>i</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>f</mi> <mo>/</mo> <msub> <mi>d</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>u</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>f</mi> <mo>/</mo> <msub> <mi>d</mi> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>v</mi> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> <mi>c</mi> </msubsup> <mrow> <mo>(</mo> <msubsup> <mi>T</mi> <mi>i</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mo>-</mo> <msubsup> <mi>T</mi> <msub> <mi>o</mi> <mi>c</mi> </msub> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mo>)</mo> </mrow> </mrow>
采用最小二乘解算得到t时刻导航坐标系nt到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置
其中，为已知的第i个特征点在导航坐标系下的坐标，为视觉传感器光学中心在导航坐标系下的位置坐标，[ui υi]T为从图像信息中获取的第i个特征点的成像在图像像素坐标系下的位置坐标，为第i个特征点到视觉传感器光学中心的距离在光轴上的投影距离，f/dx、f/dy为视觉传感器的等效焦距，u0、v0为视觉传感器光轴与图像的交点在图像像素坐标系下的位置坐标；所述导航坐标系采用横向地理坐标系gt；
步骤1.2：根据步骤1.1计算得到的导航坐标系nt到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置通过公式
以及
计算得到载体坐标系b到导航坐标系nt的转换矩阵和载体在导航坐标系下的位置坐标其中载体坐标系b到视觉传感器坐标系c的转换矩阵和平移矢量在视觉传感器进行标定后均已知；
步骤2：进行极区捷联惯导解算：
步骤2.1：建立载体的姿态，速度，位置微分方程为：
姿态微分方程：
<mrow> <msubsup> <mover> <mi>C</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> <msub> <mi>g</mi> <mi>t</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>g</mi> <mi>t</mi> </msub> </msubsup> <msubsup> <mi>&amp;Omega;</mi> <mrow> <msub> <mi>g</mi> <mi>t</mi> </msub> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> </mrow>
其中表示的叉乘反对称矩阵，为横向地理坐标系gt相对于载体坐标系b的角速度在载体坐标系b下的投影；为载体坐标系b到横向地理坐标系gt的转换矩阵；速度微分方程：
<mrow> <msup> <mover> <mi>&amp;upsi;</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>g</mi> <mi>t</mi> </msub> </msup> <mo>=</mo> <msup> <mi>f</mi> <msub> <mi>g</mi> <mi>t</mi> </msub> </msup> <mo>-</mo> <mrow> <mo>(</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <msub> <mi>eg</mi> <mi>t</mi> </msub> </mrow> <msub> <mi>g</mi> <mi>t</mi> </msub> </msubsup> <mo>+</mo> <mn>2</mn> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <msub> <mi>g</mi> <mi>t</mi> </msub> </msubsup> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <msup> <mi>&amp;upsi;</mi> <msub> <mi>g</mi> <mi>t</mi> </msub> </msup> <mo>+</mo> <msup> <mi>g</mi> <msub> <mi>g</mi> <mi>t</mi> </msub> </msup> </mrow>
其中为载体速度在横向地理坐标系gt下的投影；为载体加速度在横向地理坐标系gt下的投影；为惯性坐标系相对于地球坐标系e的角速度在横向地理坐标系gt下的投影；为地球坐标系e相对于横向地理坐标系gt的角速度在横向地理坐标系gt下的投影；为重力加速度在横向地理坐标系gt下的投影；
位置微分方程：
位置方向余弦矩阵的微分方程为：
<mrow> <msubsup> <mover> <mi>C</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>g</mi> <mi>t</mi> </msub> <msub> <mi>e</mi> <mi>t</mi> </msub> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>g</mi> <mi>t</mi> </msub> <msub> <mi>e</mi> <mi>t</mi> </msub> </msubsup> <msubsup> <mi>&amp;Omega;</mi> <mrow> <msub> <mi>e</mi> <mi>t</mi> </msub> <msub> <mi>g</mi> <mi>t</mi> </msub> </mrow> <msub> <mi>g</mi> <mi>t</mi> </msub> </msubsup> </mrow>
其中为横向地理坐标系gt到横向地球坐标系et的转换矩阵，为的叉乘反对称矩阵，为横向地球坐标系et相对于横向地理坐标系gt的角速度在横向地理坐标系gt下的投影；
载体横向纬度Lt、横向经度λt和载体高度h的微分方程为：
<mrow> <msub> <mover> <mi>L</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>t</mi> </msub> <mo>=</mo> <mfrac> <msub> <mi>&amp;upsi;</mi> <msub> <mi>N</mi> <mi>t</mi> </msub> </msub> <mrow> <msub> <mi>R</mi> <mrow> <mi>o</mi> <mi>t</mi> </mrow> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mo>,</mo> <msub> <mover> <mi>&amp;lambda;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>t</mi> </msub> <mo>=</mo> <mfrac> <msub> <mi>&amp;upsi;</mi> <msub> <mi>E</mi> <mi>t</mi> </msub> </msub> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>o</mi> <mi>t</mi> </mrow> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> <msub> <mi>cosL</mi> <mi>t</mi> </msub> </mrow> </mfrac> <mo>,</mo> <mover> <mi>h</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <msub> <mi>&amp;upsi;</mi> <msub> <mi>U</mi> <mi>t</mi> </msub> </msub> </mrow>
其中为载体在横向地理坐标系gt下的北向速度，Rot为地球的曲率半径，为载体在横向地理坐标系gt下的东向速度，为载体在横向地理坐标系gt下的天向速度；
步骤2.2：由姿态，速度，位置微分方程，按照捷联惯导导航解算方法，和为输入量，得到载体的姿态信息速度信息和位置信息，所述位置信息包括横向纬度Lt、横向经度λt；
步骤3：以步骤1结合步骤2得到的载体位置、姿态角的差值为量测值，利用卡尔曼滤波方法，估计出捷联惯导的姿态误差、速度误差和位置误差，进行组合导航滤波：
步骤3.1：滤波器的系统状态变量为：
<mrow> <mi>X</mi> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msup> <mi>&amp;phi;</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> </mtd> <mtd> <mrow> <msup> <mi>&amp;delta;V</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>&amp;delta;T</mi> <mi>b</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> </mrow> </mtd> <mtd> <msubsup> <mi>&amp;epsiv;</mi> <mi>b</mi> <mi>b</mi> </msubsup> </mtd> <mtd> <msubsup> <mo>&amp;dtri;</mo> <mi>b</mi> <mi>b</mi> </msubsup> </mtd> <mtd> <mi>&amp;theta;</mi> </mtd> <mtd> <mrow> <msubsup> <mi>&amp;delta;l</mi> <mrow> <mi>b</mi> <mi>c</mi> </mrow> <mi>b</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
其中为捷联惯导失准角，为速度误差，为位置误差，为陀螺常值漂移误差，为加速度计常值零偏误差，θ为捷联惯导与视觉辅助系统的失准角，为捷联惯导与视觉辅助系统的位置误差；
系统状态方程为：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <mover> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>=</mo> <msup> <mi>&amp;phi;</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>&amp;times;</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <msub> <mi>in</mi> <mi>t</mi> </msub> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;&amp;omega;</mi> <mrow> <msub> <mi>in</mi> <mi>t</mi> </msub> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <msubsup> <mi>&amp;delta;&amp;omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <msup> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>=</mo> <mo>-</mo> <msup> <mi>&amp;phi;</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>&amp;times;</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> <mi>b</mi> </msubsup> <msup> <mi>f</mi> <mi>b</mi> </msup> <mo>+</mo> <msup> <mi>&amp;delta;V</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>&amp;times;</mo> <mrow> <mo>(</mo> <mn>2</mn> <msubsup> <mi>&amp;omega;</mi> <mrow> <msub> <mi>ie</mi> <mi>t</mi> </msub> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <msub> <mi>e</mi> <mi>t</mi> </msub> <msub> <mi>n</mi> <mi>t</mi> </msub> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mi>V</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>&amp;times;</mo> <mrow> <mo>(</mo> <mn>2</mn> <msubsup> <mi>&amp;delta;&amp;omega;</mi> <mrow> <msub> <mi>ie</mi> <mi>t</mi> </msub> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;&amp;omega;</mi> <mrow> <msub> <mi>e</mi> <mi>t</mi> </msub> <msub> <mi>n</mi> <mi>t</mi> </msub> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> <mi>b</mi> </msubsup> <msup> <mi>&amp;delta;f</mi> <mi>b</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <msup> <mover> <mi>T</mi> <mo>&amp;CenterDot;</mo> </mover> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>=</mo> <msup> <mi>&amp;delta;V</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>&amp;epsiv;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> <mi>b</mi> </msubsup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mo>&amp;dtri;</mo> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> <mi>b</mi> </msubsup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>&amp;theta;</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <msubsup> <mover> <mi>l</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>c</mi> </mrow> <mi>b</mi> </msubsup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced>
其中nt为导航坐标系，导航坐标系nt采用横向地理坐标系gt；为惯性坐标系相对于导航坐标系nt的角速度在导航坐标系nt下的投影；为惯性坐标系相对于导航坐标系nt的角速度误差在导航坐标系nt下的投影；为惯性坐标系相对于载体坐标系b的角速度误差在载体坐标系b下的投影；为载体坐标系b到导航坐标系nt的姿态转换矩阵；fb为载体加速度在载体坐标系b下的投影；δfb为载体加速度误差在载体坐标系b下的投影，为惯性坐标系相对于横向地球坐标系et的角速度在导航坐标系nt下的投影；为横向地球坐标系et相对于导航坐标系nt的角速度在导航坐标系nt下的投影；
系统的量测方程为：
<mrow> <mi>z</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>M</mi> <mi>a</mi> <mi>t</mi> <mn>2</mn> <mi>A</mi> <mi>n</mi> <mi>g</mi> <mo>{</mo> <msubsup> <mover> <mi>C</mi> <mo>~</mo> </mover> <mi>b</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mrow> <mo>(</mo> <msubsup> <mover> <mi>C</mi> <mo>~</mo> </mover> <mi>b</mi> <mi>c</mi> </msubsup> <mo>)</mo> </mrow> <msubsup> <mover> <mi>C</mi> <mo>~</mo> </mover> <msub> <mi>n</mi> <mi>t</mi> </msub> <mi>c</mi> </msubsup> <mo>}</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>T</mi> <mi>b</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msubsup> <mi>l</mi> <mrow> <mi>b</mi> <mi>c</mi> </mrow> <mi>b</mi> </msubsup> <mo>-</mo> <msup> <mi>T</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <msup> <mi>&amp;phi;</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>+</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>c</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mi>&amp;delta;T</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mrow> <mo>(</mo> <msup> <mi>&amp;phi;</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msup> <mo>&amp;times;</mo> <mo>)</mo> </mrow> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <msubsup> <mi>l</mi> <mrow> <mi>b</mi> <mi>c</mi> </mrow> <mi>b</mi> </msubsup> <mo>+</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>n</mi> <mi>t</mi> </msub> </msubsup> <msubsup> <mi>&amp;delta;l</mi> <mrow> <mi>b</mi> <mi>c</mi> </mrow> <mi>b</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mi>c</mi> </msub> </mrow>
其中Mat2Ang{}表示姿态转换矩阵对应的姿态角，ωc为视觉姿态位置测量误差和高阶误差项引起的量测噪声；为载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影，为载体坐标系b到视觉传感器坐标系c的转换矩阵，载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；
步骤3.2：根据系统的状态方程和观测方程，利用卡尔曼滤波方法，估计出捷联惯导姿态失准角速度误差位置误差
步骤4：根据步骤3估计得到的捷联惯导姿态失准角速度误差位置误差分别对捷联惯导的输出进行修正，得到更为精确的载体姿态角，速度和位置信息。
CN201710989714.3A 2017-10-23 2017-10-23 一种视觉辅助的捷联惯导极区初始对准方法 Active CN107806874B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710989714.3A CN107806874B (zh) 2017-10-23 2017-10-23 一种视觉辅助的捷联惯导极区初始对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710989714.3A CN107806874B (zh) 2017-10-23 2017-10-23 一种视觉辅助的捷联惯导极区初始对准方法

Publications (2)

Publication Number Publication Date
CN107806874A true CN107806874A (zh) 2018-03-16
CN107806874B CN107806874B (zh) 2019-01-15

Family

ID=61592652

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710989714.3A Active CN107806874B (zh) 2017-10-23 2017-10-23 一种视觉辅助的捷联惯导极区初始对准方法

Country Status (1)

Country Link
CN (1) CN107806874B (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108663043A (zh) * 2018-05-16 2018-10-16 北京航空航天大学 基于单个相机辅助的分布式pos主子节点相对位姿测量方法
CN109099912A (zh) * 2017-08-11 2018-12-28 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN109540173A (zh) * 2018-09-17 2019-03-29 江西洪都航空工业集团有限责任公司 一种视觉辅助的传递对准方法
CN110345941A (zh) * 2019-06-13 2019-10-18 东南大学 深潜载人潜水器sins自辅助导航方法
CN110457813A (zh) * 2019-08-08 2019-11-15 西北工业大学 基于横向地理坐标系的虚拟极区方法
CN113939712A (zh) * 2019-06-06 2022-01-14 赛峰电子与防务公司 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置
CN115127547A (zh) * 2022-06-27 2022-09-30 长安大学 一种基于捷联惯导系统和图像定位的隧道检测车定位方法
CN116858287A (zh) * 2023-07-06 2023-10-10 哈尔滨工程大学 一种基于地球坐标系的极区惯导初始对准方法
CN117470234A (zh) * 2023-11-10 2024-01-30 中国人民解放军国防科技大学 基于Psi角误差模型的舰船跨极区滤波切换方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2131147A1 (en) * 2008-05-15 2009-12-09 Honeywell International Inc. systems and methods for improving magnetic heading output from an inertial navigation system
CN103940429A (zh) * 2014-03-21 2014-07-23 哈尔滨工程大学 一种惯性导航系统横坐标系下载体姿态的实时测量方法
CN105973268A (zh) * 2016-05-06 2016-09-28 哈尔滨工程大学 一种基于共基座安装的传递对准精度定量评估方法
CN106482749A (zh) * 2016-12-07 2017-03-08 西北工业大学 基于逆向导航算法的捷联惯导与转速计组合对准方法
CN107063246A (zh) * 2017-04-24 2017-08-18 齐鲁工业大学 一种视觉导航/惯性导航的松散组合导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2131147A1 (en) * 2008-05-15 2009-12-09 Honeywell International Inc. systems and methods for improving magnetic heading output from an inertial navigation system
CN103940429A (zh) * 2014-03-21 2014-07-23 哈尔滨工程大学 一种惯性导航系统横坐标系下载体姿态的实时测量方法
CN105973268A (zh) * 2016-05-06 2016-09-28 哈尔滨工程大学 一种基于共基座安装的传递对准精度定量评估方法
CN106482749A (zh) * 2016-12-07 2017-03-08 西北工业大学 基于逆向导航算法的捷联惯导与转速计组合对准方法
CN107063246A (zh) * 2017-04-24 2017-08-18 齐鲁工业大学 一种视觉导航/惯性导航的松散组合导航方法

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109099912B (zh) * 2017-08-11 2022-05-10 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN109099912A (zh) * 2017-08-11 2018-12-28 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN108663043A (zh) * 2018-05-16 2018-10-16 北京航空航天大学 基于单个相机辅助的分布式pos主子节点相对位姿测量方法
CN108663043B (zh) * 2018-05-16 2020-01-10 北京航空航天大学 基于单个相机辅助的分布式pos主子节点相对位姿测量方法
CN109540173A (zh) * 2018-09-17 2019-03-29 江西洪都航空工业集团有限责任公司 一种视觉辅助的传递对准方法
CN113939712B (zh) * 2019-06-06 2023-11-28 赛峰电子与防务公司 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置
CN113939712A (zh) * 2019-06-06 2022-01-14 赛峰电子与防务公司 基于运输设备取景器传送的信息重置运输设备惯性单元的方法和装置
CN110345941A (zh) * 2019-06-13 2019-10-18 东南大学 深潜载人潜水器sins自辅助导航方法
CN110345941B (zh) * 2019-06-13 2022-08-19 东南大学 深潜载人潜水器sins自辅助导航方法
CN110457813A (zh) * 2019-08-08 2019-11-15 西北工业大学 基于横向地理坐标系的虚拟极区方法
CN115127547A (zh) * 2022-06-27 2022-09-30 长安大学 一种基于捷联惯导系统和图像定位的隧道检测车定位方法
CN115127547B (zh) * 2022-06-27 2024-04-19 长安大学 一种基于捷联惯导系统和图像定位的隧道检测车定位方法
CN116858287A (zh) * 2023-07-06 2023-10-10 哈尔滨工程大学 一种基于地球坐标系的极区惯导初始对准方法
CN117470234A (zh) * 2023-11-10 2024-01-30 中国人民解放军国防科技大学 基于Psi角误差模型的舰船跨极区滤波切换方法
CN117470234B (zh) * 2023-11-10 2024-04-26 中国人民解放军国防科技大学 基于Psi角误差模型的舰船跨极区滤波切换方法

Also Published As

Publication number Publication date
CN107806874B (zh) 2019-01-15

Similar Documents

Publication Publication Date Title
CN107806874A (zh) 一种视觉辅助的捷联惯导极区初始对准方法
CN108318052B (zh) 一种基于双轴连续旋转的混合式平台惯导系统标定方法
CN110501024A (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN103994763B (zh) 一种火星车的sins/cns深组合导航系统及其实现方法
CN103697889B (zh) 一种基于多模型分布式滤波的无人机自主导航与定位方法
CN106708066A (zh) 基于视觉/惯导的无人机自主着陆方法
CN106500731B (zh) 一种基于恒星模拟系统的船载经纬仪的标校方法
CN105203098A (zh) 基于九轴mems传感器的农业机械全姿态角更新方法
CN108375383B (zh) 多相机辅助的机载分布式pos柔性基线测量方法和装置
CN109269512B (zh) 行星着陆图像与测距融合的相对导航方法
CN103196448A (zh) 一种机载分布式惯性测姿系统及其传递对准方法
CN105444781B (zh) 星载自主引导成像地面验证方法
CN104713555A (zh) 应用全天域中性点辅助定向的车辆自主导航方法
CN106885571B (zh) 一种结合imu和导航影像的月面巡视器快速定位方法
CN104655135B (zh) 一种基于地标识别的飞行器视觉导航方法
CN109211269A (zh) 一种双轴旋转惯导系统姿态角误差标定方法
CN105973268B (zh) 一种基于共基座安装的传递对准精度定量评估方法
CN114061623B (zh) 一种基于双天线测向的惯性传感器零偏误差辨识方法
CN106352897B (zh) 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN105021212B (zh) 一种初始方位信息辅助下潜航器快速传递对准方法
CN106672265A (zh) 一种基于光流信息的小天体定点着陆制导控制方法
CN110044361A (zh) 基于目标投影位置的光学载荷星上自主调度方法
CN109506662B (zh) 一种小天体着陆初始对准方法、其相对导航基准确定方法及装置
Zhang et al. Self-calibration of the stereo vision system of the Chang'e-4 Lunar rover based on the points and lines combined adjustment

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