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

Abstract

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

Description

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

技术领域

本发明涉及捷联惯导的极区初始对准技术，涉及一种捷联惯导/视觉组合导航滤波方法，实现极区初始对准。

背景技术

由于地球自转角速度矢量和重力加速度矢量之间的夹角随纬度的增加而减小，直至重合，所以在极区捷联惯导自对准实现困难，须借助其他方法完成初始对准。

借助主惯导系统的传递对准是目前捷联惯导在极区进行初始对准的主要方法。在传递对准过程中，主惯导必须处于导航状态，将其速度输出看作无误差，并且主惯导的精度应比待对准的捷联惯导高，通过速度匹配和卡尔曼滤波技术，以主惯导与待对准的捷联惯导的速度差为量测值估计出失准角，估计出待对准的捷联惯导姿态误差角。

传递对准方法虽然可以实现捷联惯导在极区的初始对准，但是必须依靠高精度的主惯导才能完成，并且主惯导必须处于导航状态，所以其应用有很大的限制条件。

发明内容

为解决现有技术存在的问题，本发明提出一种视觉辅助的捷联惯导极区初始对准方法，将捷联惯导与视觉测量相结合，通过与捷联惯导固连的视觉测量系统测量位置、姿态已知的固定在地面上的标志物，实现了捷联惯导在极区的初始对准，由于其不依靠其他高精度的导航设备(比如说不需要高精度的主惯导)，所以可以应用在更多的场合下。

本发明的技术方案为：

所述一种视觉辅助的捷联惯导极区初始对准方法，其特征在于：包括以下步骤：

步骤1：进行视觉辅助导航解算：

步骤1.1：采用视觉传感器获取t时刻的图像，并从图像中提取至少6个特征点，将特征点信息代入以下公式

采用最小二乘解算得到t时刻导航坐标系n_t到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置

其中，为已知的第i个特征点在导航坐标系下的坐标，为视觉传感器光学中心在导航坐标系下的位置坐标，[u_i υ_i]^T为从图像信息中获取的第i个特征点的成像在图像像素坐标系下的位置坐标，为第i个特征点到视觉传感器光学中心的距离在光轴上的投影距离，f/d_x、f/d_y为视觉传感器的等效焦距，u₀、v₀为视觉传感器光轴与图像的交点在图像像素坐标系下的位置坐标；所述导航坐标系采用横向地理坐标系g_t；

步骤1.2：根据步骤1.1计算得到的导航坐标系n_t到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置通过公式

以及

计算得到载体坐标系b到导航坐标系n_t的转换矩阵和载体在导航坐标系下的位置坐标其中载体坐标系b到视觉传感器坐标系c的转换矩阵和载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影在视觉传感器进行标定后均已知；

步骤2：进行极区捷联惯导解算：

步骤2.1：建立载体的姿态，速度，位置微分方程为：

姿态微分方程：

其中表示的叉乘反对称矩阵，为横向地理坐标系g_t相对于载体坐标系b的角速度在载体坐标系b下的投影；为载体坐标系b到横向地理坐标系g_t的转换矩阵；

速度微分方程：

其中为载体速度在横向地理坐标系g_t下的投影；为载体加速度在横向地理坐标系g_t下的投影；为惯性坐标系相对于地球坐标系e的角速度在横向地理坐标系g_t下的投影；为地球坐标系e相对于横向地理坐标系g_t的角速度在横向地理坐标系g_t下的投影；为重力加速度在横向地理坐标系g_t下的投影；

位置微分方程：

位置方向余弦矩阵的微分方程为：

其中为横向地理坐标系g_t到横向地球坐标系e_t的转换矩阵，为的叉乘反对称矩阵，为横向地球坐标系e_t相对于横向地理坐标系g_t的角速度在横向地理坐标系g_t下的投影；

载体横向纬度L_t、横向经度λ_t和载体高度h的微分方程为：

其中为载体在横向地理坐标系g_t下的北向速度，R_ot为地球的曲率半径，为载体在横向地理坐标系g_t下的东向速度，为载体在横向地理坐标系g_t下的天向速度；

步骤2.2：由姿态，速度，位置微分方程，按照捷联惯导导航解算方法，和为输入量，得到载体的姿态信息速度信息和位置信息，所述位置信息包括横向纬度L_t、横向经度λ_t；

步骤3：以步骤1结合步骤2得到的载体位置、姿态角的差值为量测值，利用卡尔曼滤波方法，估计出捷联惯导的姿态误差、速度误差和位置误差，进行组合导航滤波：

步骤3.1：滤波器的系统状态变量为：

其中为捷联惯导失准角，为速度误差，为位置误差，为陀螺常值漂移误差，为加速度计常值零偏误差，θ为捷联惯导与视觉辅助系统的失准角，为载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；

系统状态方程为：

其中n_t为导航坐标系，导航坐标系n_t采用横向地理坐标系g_t；为惯性坐标系相对于导航坐标系n_t的角速度在导航坐标系n_t下的投影；为惯性坐标系相对于导航坐标系n_t的角速度误差在导航坐标系n_t下的投影；为惯性坐标系相对于载体坐标系b的角速度误差在载体坐标系b下的投影；为载体坐标系b到导航坐标系n_t的姿态转换矩阵；f^b为载体加速度在载体坐标系b下的投影；δf^b为载体加速度误差在载体坐标系b下的投影，为惯性坐标系相对于横向地球坐标系e_t的角速度在导航坐标系n_t下的投影；为横向地球坐标系e_t相对于导航坐标系n_t的角速度在导航坐标系n_t下的投影；

系统的量测方程为：

其中Mat2Ang{}表示姿态转换矩阵对应的姿态角，ω_c为视觉姿态位置测量误差和高阶误差项引起的量测噪声；为载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影，为载体坐标系b到视觉传感器坐标系c的转换矩阵，为载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；

步骤3.2：根据系统的状态方程和观测方程，利用卡尔曼滤波方法，估计出捷联惯导姿态失准角速度误差位置误差

步骤4：根据步骤3估计得到的捷联惯导姿态失准角速度误差位置误差分别对捷联惯导的输出进行修正，得到更为精确的载体姿态角，速度和位置信息。

有益效果

本发明通过对计算机视觉系统的引入，提出了一种视觉辅助的捷联惯导极区初始对准方法，解决了捷联惯导无法在极区自对准的问题，并且不依靠其他高精度导航设备，应用场合更加广泛。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为本发明视觉辅助的捷联惯导极区初始对准方法流程图。

图2为横向地球坐标系。

图3为图像的三种坐标系。

图4为视觉辅助系统原理说明。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

首先结合附图3对视觉系统上定义三个坐标系进行说明：

图像像素坐标系：

图像像素坐标系是以图像平面左上角像元为原点，u轴和v轴分别对应成像平面的行数和列数，(u,v)表示以像素为单位的图像像素坐标。

图像平面坐标系：

图像平面与光轴垂直，(x_p,y_p)表示以毫米为单位的坐标。在x_p，y_p坐标系中，原点O_p定义在图像平面与相机光轴的交点，并位于图像中心。

视觉传感器坐标系：

视觉传感器坐标系的原点定义在视觉传感器的光心O_c处，O_cY_c与光轴重合指向前方，O_cZ_c轴与图像平面坐标系中的O_pY_p平行，O_cX_c与图像平面坐标系中的O_pX_p轴平行。

下面结合附图4对本发明原理进行说明。

本发明包含四个步骤，也可以认为是由视觉辅助模块、捷联惯导极区解算模块、组合导航滤波模块、反馈校正输出模块四部分组成。

步骤1：进行视觉辅助导航解算：

步骤1.1：采用视觉传感器获取t时刻的图像，并从图像中提取至少6个特征点，将特征点信息代入以下公式

采用最小二乘解算得到t时刻导航坐标系n_t到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置所述导航坐标系采用横向地理坐标系g_t；

其中，为已知的第i个特征点在导航坐标系下的坐标，为视觉传感器光学中心在导航坐标系下的位置坐标，[u_i υ_i]^T为从图像信息中获取的第i个特征点的成像在图像像素坐标系下的位置坐标，为第i个特征点到视觉传感器光学中心的距离在光轴上的投影距离，f/d_x、f/d_y为视觉传感器的等效焦距，u₀、v₀为视觉传感器光轴与图像的交点在图像像素坐标系下的位置坐标；对于已经标定过的视觉传感器，f、d_x、d_y、u₀、v₀为已知量。

步骤1.2：根据步骤1.1计算得到的导航坐标系n_t到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置通过公式

以及

计算得到载体坐标系b到导航坐标系n_t的转换矩阵和载体在导航坐标系下的位置坐标由于视觉传感器和载体固连，所以视觉传感器标定后，载体坐标系b到视觉传感器坐标系c的转换矩阵和载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影在视觉传感器进行标定后均已知。

步骤2：进行极区捷联惯导解算：

以90°E/W经线圈作为伪赤道，以0°/180°经线圈作为伪本初子午圈，赤道与伪本初子午圈的交点为伪极点，这就被称为伪地球坐标系或者称为横向地球坐标系。在横向地球坐标系下进行导航解算，克服了真实地球极点处经线收敛带来的定向和定位困难。

由横向地球坐标系的定义，地球坐标系e绕oy_e转动-90°即可获得横向地球坐标系e_t，根据基本旋转理论可得地球坐标系e到横向地球坐标系e_t的转换关系为：

载体在地理坐标系和横向地理坐标系下的位置转换关系为：

L_t＝-arcsin(cos L cosλ)

L＝arcsin(cos L_tcosλ_t)

L_t为载体在横向地理坐标系下的纬度，λ_t为载体在横向地理坐标系下的经度，L为载体在地理坐标系下的纬度，λ为载体在地理坐标系下的经度

地理坐标系g到横向地理坐标系g_t的转换关系为：

为横向地球坐标系e_t到横向地理坐标系g_t的转换矩阵，为地理坐标系g到地球坐标系e的转换矩阵；

其中：

横向惯性导航将横向地理坐标系g_t作为导航坐标系n_t进行解算，其力学编排与指北方位惯导系统相似。

步骤2.1：建立载体的姿态，速度，位置微分方程为：

姿态微分方程：

其中表示的叉乘反对称矩阵，为横向地理坐标系g_t相对于载体坐标系b的角速度在载体坐标系b下的投影；为载体坐标系b到横向地理坐标系g_t的转换矩阵；

速度微分方程：

其中为载体速度在横向地理坐标系g_t下的投影；为载体加速度在横向地理坐标系g_t下的投影；为惯性坐标系相对于地球坐标系e的角速度在横向地理坐标系g_t下的投影；为地球坐标系e相对于横向地理坐标系g_t的角速度在横向地理坐标系g_t下的投影；为重力加速度在横向地理坐标系g_t下的投影；

位置微分方程：

在惯性导航系统中将横向地理坐标系作为导航坐标系，位置方向余弦矩阵的微分方程为：

其中为横向地理坐标系g_t到横向地球坐标系e_t的转换矩阵，为的叉乘反对称矩阵，为横向地球坐标系e_t相对于横向地理坐标系g_t的角速度在横向地理坐标系g_t下的投影；

载体横向纬度L_t、横向经度λ_t和载体高度h的微分方程为：

其中为载体在横向地理坐标系g_t下的北向速度，R_ot为地球的曲率半径，为载体在横向地理坐标系g_t下的东向速度，为载体在横向地理坐标系g_t下的天向速度；

步骤2.2：由姿态，速度，位置微分方程，按照捷联惯导导航解算方法，和为输入量，得到载体的姿态信息速度信息和位置信息，所述位置信息包括横向纬度L_t、横向经度λ_t。

步骤3：以步骤1结合步骤2得到的载体位置、姿态角的差值为量测值，利用卡尔曼滤波方法，估计出捷联惯导的姿态误差、速度误差和位置误差，进行组合导航滤波：

步骤3.1：滤波器的系统状态变量为：

其中为捷联惯导失准角，为速度误差，为位置误差，为陀螺常值漂移误差，为加速度计常值零偏误差，θ为捷联惯导与视觉辅助系统的失准角，为载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；

系统状态方程为：

其中n_t为导航坐标系，导航坐标系n_t采用横向地理坐标系g_t；为惯性坐标系相对于导航坐标系n_t的角速度在导航坐标系n_t下的投影；为惯性坐标系相对于导航坐标系n_t的角速度误差在导航坐标系n_t下的投影；为惯性坐标系相对于载体坐标系b的角速度误差在载体坐标系b下的投影；为载体坐标系b到导航坐标系n_t的姿态转换矩阵；f^b为载体加速度在载体坐标系b下的投影；δf^b为载体加速度误差在载体坐标系b下的投影，为惯性坐标系相对于横向地球坐标系e_t的角速度在导航坐标系n_t下的投影；为横向地球坐标系e_t相对于导航坐标系n_t的角速度在导航坐标系n_t下的投影；

系统的量测方程为：

其中Mat2Ang{}表示姿态转换矩阵对应的姿态角，ω_c为视觉姿态位置测量误差和高阶误差项引起的量测噪声；为载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影，为载体坐标系b到视觉传感器坐标系c的转换矩阵，为载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；

在上述的组合模式中，其量测值z有两组：一组为位置量测差值，另一组为姿态量测差值；所谓位置量测差值是由捷联惯导系统给出的经度、纬度、高度信息和视觉辅助系统给出的相应的信息求差，作为一组量测值；所谓姿态量测差值是由捷联惯导系统给出的姿态信息和视觉辅助系统给出的姿态信息求差，作为一组量测值；

步骤3.2：根据系统的状态方程和观测方程，利用卡尔曼滤波方法，估计出捷联惯导姿态失准角速度误差位置误差

步骤4：根据步骤3估计得到的捷联惯导姿态失准角速度误差位置误差分别对捷联惯导的输出进行修正，得到更为精确的载体姿态角，速度和位置信息。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

1.一种视觉辅助的捷联惯导极区初始对准方法，其特征在于：包括以下步骤：

步骤1：进行视觉辅助导航解算：

步骤1.1：采用视觉传感器获取t时刻的图像，并从图像中提取至少6个特征点，将特征点信息代入以下公式

采用最小二乘解算得到t时刻导航坐标系n_t到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置

其中，为已知的第i个特征点在导航坐标系下的坐标，为视觉传感器光学中心在导航坐标系下的位置坐标，[u_i υ_i]^T为从图像信息中获取的第i个特征点的成像在图像像素坐标系下的位置坐标，为第i个特征点到视觉传感器光学中心的距离在光轴上的投影距离，f/d_x、f/d_y为视觉传感器的等效焦距，u₀、v₀为视觉传感器光轴与图像的交点在图像像素坐标系下的位置坐标；所述导航坐标系采用横向地理坐标系g_t；

步骤1.2：根据步骤1.1计算得到的导航坐标系n_t到视觉传感器坐标系c的转换矩阵和视觉传感器在导航坐标系下的位置通过公式

以及

计算得到载体坐标系b到导航坐标系n_t的转换矩阵和载体在导航坐标系下的位置坐标其中载体坐标系b到视觉传感器坐标系c的转换矩阵和载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影在视觉传感器进行标定后均已知；

步骤2：进行极区捷联惯导解算：

步骤2.1：建立载体的姿态，速度，位置微分方程为：

姿态微分方程：

其中表示的叉乘反对称矩阵，为横向地理坐标系g_t相对于载体坐标系b的角速度在载体坐标系b下的投影；为载体坐标系b到横向地理坐标系g_t的转换矩阵；

速度微分方程：

其中为载体速度在横向地理坐标系g_t下的投影；为载体加速度在横向地理坐标系g_t下的投影；为惯性坐标系相对于地球坐标系e的角速度在横向地理坐标系g_t下的投影；为地球坐标系e相对于横向地理坐标系g_t的角速度在横向地理坐标系g_t下的投影；为重力加速度在横向地理坐标系g_t下的投影；

位置微分方程：

位置方向余弦矩阵的微分方程为：

其中为横向地理坐标系g_t到横向地球坐标系e_t的转换矩阵，为的叉乘反对称矩阵，为横向地球坐标系e_t相对于横向地理坐标系g_t的角速度在横向地理坐标系g_t下的投影；

载体横向纬度L_t、横向经度λ_t和载体高度h的微分方程为：

其中为载体在横向地理坐标系g_t下的北向速度，R_ot为地球的曲率半径，为载体在横向地理坐标系g_t下的东向速度，为载体在横向地理坐标系g_t下的天向速度；

步骤2.2：由姿态，速度，位置微分方程，按照捷联惯导导航解算方法，和为输入量，得到载体的姿态信息速度信息和位置信息，所述位置信息包括横向纬度L_t、横向经度λ_t；

步骤3：以步骤1结合步骤2得到的载体位置、姿态角的差值为量测值，利用卡尔曼滤波方法，估计出捷联惯导的姿态误差、速度误差和位置误差，进行组合导航滤波：

步骤3.1：滤波器的系统状态变量为：

其中为捷联惯导失准角，为速度误差，为位置误差，为陀螺常值漂移误差，为加速度计常值零偏误差，θ为捷联惯导与视觉辅助系统的失准角，为载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；

系统状态方程为：

其中n_t为导航坐标系，导航坐标系n_t采用横向地理坐标系g_t；为惯性坐标系相对于导航坐标系n_t的角速度在导航坐标系n_t下的投影；为惯性坐标系相对于导航坐标系n_t的角速度误差在导航坐标系n_t下的投影；为惯性坐标系相对于载体坐标系b的角速度误差在载体坐标系b下的投影；为载体坐标系b到导航坐标系n_t的姿态转换矩阵；f^b为载体加速度在载体坐标系b下的投影；δf^b为载体加速度误差在载体坐标系b下的投影，为惯性坐标系相对于横向地球坐标系e_t的角速度在导航坐标系n_t下的投影；为横向地球坐标系e_t相对于导航坐标系n_t的角速度在导航坐标系n_t下的投影；

系统的量测方程为：

其中Mat2Ang{}表示姿态转换矩阵对应的姿态角，ω_c为视觉姿态位置测量误差和高阶误差项引起的量测噪声；为载体坐标系b到视觉传感器坐标系c的平移矢量在载体坐标系b系下的投影，为载体坐标系b到视觉传感器坐标系c的转换矩阵，载体坐标系b到视觉传感器坐标系c的平移矢量误差在载体坐标系b系下的投影；

步骤3.2：根据系统的状态方程和观测方程，利用卡尔曼滤波方法，估计出捷联惯导姿态失准角速度误差位置误差

步骤4：根据步骤3估计得到的捷联惯导姿态失准角速度误差位置误差分别对捷联惯导的输出进行修正，得到更为精确的载体姿态角，速度和位置信息。