[發明專利]一種基于視覺的無人機伺服跟蹤方法

有效

• 申請號: 202110493964.4
• 申請日: 2021-05-07
• 公開（公告）號: CN113311873B
• 公開（公告）日: 2022-09-06
• 發明（設計）人: 何玉慶;楊麗英;黃朝雄;狄春雷;于利;王昕媛
• 申請（專利權）人: 中國科學院沈陽自動化研究所
• 主分類號: G05D1/12
• 分類號: G05D1/12;G01C11/04
• 代理公司: 沈陽科苑專利商標代理有限公司 21002
• 代理人: 周宇

1.一種基于視覺的無人機伺服跟蹤方法，其特征在于，包括如下步驟：

步驟1、對攝像頭進行標定與校正、建立背景模板圖庫；

步驟2、將雙目相機拍攝待檢測圖像與圖庫中背景模板圖像進行差分運算提取前景目標車輛圖像，記錄圖像坐標系下目標車輛任一點的二維像素坐標值(u，v)；

步驟3、建立世界坐標系、相機坐標系、圖像坐標系，獲取坐標系之間的轉換關系；

步驟4、根據三角測量法和相機投影方程，將圖像坐標系下目標車輛任一點的二維像素坐標值(u，v)轉換為相機坐標系下的任一點P_c在左相機坐標系的坐標；

步驟5、根據坐標系之間的轉換關系，將相機坐標系下的任一點P_c在左相機坐標系的坐標轉換為該特征點P_c在世界坐標系下坐標值P_w；

步驟6、根據獲得的世界坐標系下目標車輛的車頭或車尾坐標獲取無人機與目標車輛的車頭或車尾的距離，根據目標車輛幾何參數以及三角函數原理，獲取目標車輛行進航向Yaw_L；

步驟7、根據移動車輛經緯度位置信息[Lat_L,Lon_L]、移動車輛速度信息Vel_L以及計算的目標車輛行進航向Yaw_L，優化無人機下一時刻的經緯度位置期望；包括：

[Lat_sp_F,Lon_sp_F]

＝[Lat_L,Lon_L]+Dist*[sin(Yaw_L),cos(Yaw_L)]+Vel_L

*[cos(Yaw_L),sin(Yaw_L)]

其中，Dist為無人機與被測的目標移動車輛幾何中心的實際距離；

步驟8、無人機下一時刻的經緯度位置期望與無人機的實時經緯度位置輸入PID控制器生成位置增量給動力執行器，從而調整無人機飛行軌跡位置完成無人機對被測目標車輛的伺服跟隨。

2.根據權利要求1所述的一種基于視覺的無人機伺服跟蹤方法，其特征在于，所述建立背景模板圖庫包括：采集包含待檢測車輛的多張圖像，設置參數閾值獲取背景模板并存儲在背景圖庫并實時更新。

3.根據權利要求1所述的一種基于視覺的無人機伺服跟蹤方法，其特征在于，是采用Bouguet方法校正攝像頭，使攝像頭光軸平行并相交于無窮遠。

4.根據權利要求1所述的一種基于視覺的無人機伺服跟蹤方法，其特征在于，所述坐標系之間的轉換關系，包括：

選用左相機坐標系，坐標軸定義如下：X軸與圖像坐標系u軸平行，Y軸與圖像坐標系v軸平行，光軸與鏡頭光心重合，攝像頭視角內空間任意一點P_c(x_c，y_c，z_c)與圖像平面對應關系可表示為：

其中，z_c是像機坐標系下z軸坐標；u、v是攝像頭拍攝得到的圖像坐標系下像素坐標值；d_x、d_y是圖像坐標系下每一個像素在X軸和Y軸方向的物理尺寸，單位為mm；u₀、v₀是攝像頭光軸與圖像平面的交點坐標值；F是相機焦距；令a_x＝F/d_x，a_y＝F/d_y；R，t分別是世界坐標系相對于相機坐標系的旋轉矩陣和平移向量；P_w(x_w，y_w，z_w)是相機坐標系下的特征點P_c在世界坐標系下坐標值；M稱為投影矩陣，而A和[R|t]分別為攝像頭的內參和外參，上標～表示齊次向量。