[發明專利]一種視覺輔助的捷聯慣導極區初始對準方法

有效

• 申請號: 201710989714.3
• 申請日: 2017-10-23
• 公開（公告）號: CN107806874B
• 公開（公告）日: 2019-01-15
• 發明（設計）人: 張福斌;宋保維;劉明雍;張立川;嚴衛生;彭星光
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 陳星

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估計得到的捷聯慣導姿態失準角速度誤差位置誤差分別對捷聯慣導的輸出進行修正，得到更為精確的載體姿態角，速度和位置信息。