[发明专利]一种视觉辅助的捷联惯导极区初始对准方法

摘要

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

主权项

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下的投影；载体横向纬度Lt、横向经度λ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估计得到的捷联惯导姿态失准角速度误差位置误差分别对捷联惯导的输出进行修正，得到更为精确的载体姿态角，速度和位置信息。