[發明專利]一種面向無人機的視覺/激光測距高空導航方法

在審

• 申請號: 202211113811.3
• 申請日: 2022-09-14
• 公開（公告）號: CN115560760A
• 公開（公告）日: 2023-01-03
• 發明（設計）人: 尚克軍;李至;扈光鋒;莊廣琛;劉崇亮;王大元;李茜茜;王海軍;裴新凱;焦浩;明麗;段昊雨
• 申請（專利權）人: 北京自動化控制設備研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06T7/73;G06T7/521
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種面向無人機的視覺/激光測距高空導航方法，其特征在于，所述方法包括：

利用激光測距和視覺三角化獲取初始尺度估計值；

基于視覺特征匹配結果構造視覺重投影誤差方程，根據激光測距機與視覺觀測量的關聯構造激光測距誤差方程，并聯合兩個方程以獲取視覺重投影誤差和激光測距誤差聯合優化函數，所述聯合優化函數包括待優化參數：尺度因子、位姿轉換矩陣和世界坐標系下的三維地圖點；

基于所述初始尺度估計值，采用圖優化方式對所述聯合優化函數進行求解以獲取優化參數；

根據優化參數更新整個視覺跟蹤線程，完成尺度更新和位姿更新，獲取無人機的精準定位。

2.根據權利要求1所述的一種面向無人機的視覺/激光測距高空導航方法，其特征在于，所述利用激光測距和視覺三角化獲取初始尺度估計值，包括：

利用視覺傳感器在高空平飛階段進行初始跟蹤以創建關鍵幀；

利用三角化技術獲取第i個關鍵幀第j′個特征點對應的三維地圖點的深度值d_j′，0≤j′≤n；

根據所述三維地圖點的深度值獲取第i個關鍵幀對應的三維地圖點的平均深度depth_i：

接受激光測距的輸入值，將其時間戳與關鍵幀圖像時間戳對齊，經過時間戳篩選獲取第i個關鍵幀對應的激光測距高度height_i；

根據所述激光測距高度height_i和平均深度depth_i獲取第i個關鍵幀對應的初始尺度估計值s_begin：

3.根據權利要求1或2所述的一種面向無人機的視覺/激光測距高空導航方法，其特征在于，通過下述方式基于視覺特征匹配結果構造視覺重投影誤差方程：

對于前n個關鍵幀，重投影誤差表示為下式：

其中，j表示地圖點ID，表示世界坐標系下第j個地圖點的三維坐標，p_ij表示第i個關鍵圖像幀上對應的像素觀測點，K表示相機內參矩陣，為位姿矩陣，表示從第i個關鍵幀對應的世界坐標系向相機坐標系的轉換，π為深度。

4.根據權利要求3所述的一種面向無人機的視覺/激光測距高空導航方法，其特征在于，通過下述方式根據激光測距機與視覺觀測量的關聯構造激光測距誤差方程：

其中，代表激光測距誤差，s代指尺度因子，表示激光方向區域的世界坐標系下第j個地圖點的三維坐標，代表第i個關鍵幀對應的相機坐標系到世界坐標系下的轉換，簡化表示為其中旋轉矩陣為R_wc，平移向量為t_wc，代表相機坐標系下激光測距方向的向量，L_i表示激光輸出長度。

5.根據權利要求4所述的一種面向無人機的視覺/激光測距高空導航方法，其特征在于，通過下式獲取視覺重投影誤差和激光測距誤差聯合優化函數：

其中，為視覺重投影誤差，為激光測距誤差，m代指視覺重投影中用到的地圖點數目最大值，m'代指視覺重投影中用到的地圖點數目最大值，由于激光具有方向性，因此m'≤m，y_k為待優化參數，包括尺度因子s，位姿轉換矩陣T_cw，可以表示為T_cw＝[R_cw|t_cw]，和世界坐標系下的三維地圖點

6.根據權利要求5所述的一種面向無人機的視覺/激光測距高空導航方法，其特征在于，基于所述初始尺度估計值，采用圖優化方式對所述聯合優化函數進行求解以獲取優化參數，包括：

1)根據所述聯合優化函數建立新的圖優化模型，定義模型節點、邊和節點更新方式，并計算優化參數的雅可比矩陣；

2)基于所述新的圖優化模型和優化參數的雅可比矩陣，采用迭代求解的方式獲取優化后的尺度因子、三維地圖點坐標和視覺關鍵幀位姿量，其中，設置待優化參數尺度的初始賦值為所述初始尺度估計值。