[發明專利]一種2自由度吊艙的外參標定優化方法

有效

• 申請號: 201810424166.4
• 申請日: 2018-05-07
• 公開（公告）號: CN108873031B
• 公開（公告）日: 2020-09-18
• 發明（設計）人: 周晗;方強;唐鄧清;周勇;王樹源;曹正江;高平海;趙框;周正元;王熙童
• 申請（專利權）人: 中國人民解放軍國防科技大學
• 主分類號: G01S19/42
• 分類號: G01S19/42;G01S19/53;G01C21/00
• 代理公司: 長沙國科天河知識產權代理有限公司 43225
• 代理人: 董惠文

1.一種2自由度吊艙的外參標定優化方法，其特征在于：包括以下步驟：

(1)給定無人機一條標定優化數據采集的飛行航跡，無人機采用盤旋繞圈的模式進行按照給定的飛行軌跡飛行，無人機其吊艙能夠實時的跟蹤地面目標；地面目標上安裝有GPS傳感器且地面目標靜止不動；

實時獲取無人機的GPS信息即無人機在世界坐標系中的空間位置Pos_w＝[x_w,y_w,z_w]、無人機機載坐標系到世界坐標系的旋轉矩陣和吊艙反饋的姿態角(φ,θ,0)；

(2)獲得地面目標空間位置Pos_T＝[x_T,y_T,z_T]；

(3)假設地面目標中心位置在吊艙中成像位置為(u,v)，根據目標投影原理、步驟(1)中得到的無人機的GPS信息和步驟(1)中得到的吊艙反饋的姿態角，地面目標相對于無人機的距離在世界坐標系中的投影(x_w,y_w,z_w)估計如下:

其中為吊艙中的攝像機坐標系到世界坐標系的旋轉矩陣；為初始時刻吊艙坐標系到無人機機載坐標系的旋轉矩陣，對應的歐拉角為為吊艙坐標系從初始時刻到t_k時刻的旋轉矩陣，可以實時獲??；為攝像機坐標系與吊艙坐標系的旋轉矩陣，這個旋轉矩陣是固定不變的，對應的歐拉角為(u₀,v₀)為相機主點坐標；(f_x,f_y)分別為(u,v)方向上對應的有效焦距；z_c為目標在光軸的投影；

因此需要優化的外參為和

(4)定義如下能量函數

其中N為采集的數量，無人機在步驟(1)中的飛行采集過程中，檢測到地面目標的次數即為采集的數量N，(x_w,y_w,z_w)為無人機在世界坐標系中的空間位置，(x_T,y_T,z_T)為步驟(2)中得到的地面目標空間位置；

根據公式(1)和公式(2)，采用Matlab中的fminsearch(fun,x0)優化函數來進行外參優化，其中fun為公式(2)，x0為初始輸入，包含初始的輸入外參姿態角和為吊艙上電時刻的角度輸出值，為零向量，最終獲得的結果和就是外參姿態標定優化的結果，從而可以利用公式(1)實時的解算地面目標的空間位置。

2.根據權利要求1所述的2自由度吊艙的外參標定優化方法，其特征在于：無人機為固定翼或者旋翼無人機。

3.根據權利要求1或2所述的2自由度吊艙的外參標定優化方法，其特征在于：步驟(2)中，基于GPS傳感器采集一段時間內地面目標實時空間位置數據，然后取平均，即得到地面目標空間位置Pos_T＝[x_T,y_T,z_T]。