[發明專利]一種動態戰場環境實時路徑規劃系統及方法

有效

• 申請號: 202011373402.8
• 申請日: 2020-11-30
• 公開（公告）號: CN112509056B
• 公開（公告）日: 2022-12-20
• 發明（設計）人: 張西山;連光耀;李會杰;閆鵬程;孫江生;袁祥波;連云峰;代冬升;李雅峰;李萬領;鐘華;付久長;王子林;宋秦松;郭偉
• 申請（專利權）人: 中國人民解放軍32181部隊
• 主分類號: G06T7/80
• 分類號: G06T7/80;G06T17/05;G06T5/00;G01C21/20;G01C21/00;G06N3/04
• 代理公司: 河北冀華知識產權代理有限公司 13151
• 代理人: 侯迎新

1.一種動態戰場環境實時路徑規劃系統，其特征在于，包括：地圖重建模塊、視覺感知模塊、智能決策模塊以及UI交互模塊；

其中，所述UI交互模塊實現對不同模塊間參數的相互調用和信息連接；

所述地圖重建模塊包括：視覺SLAM以無人機在線拍攝的航拍影像為輸入，實時為每一幀影像生成3D點云，3D點云結合影像的紋理信息實時生成局部DSM，并將局部DSM融合到全局DSM生成實時數字地表模型；

視覺感知模塊用以提取無人機拍攝的影像中存在的地理實體，將實時數字地表模型生成柵格地圖，并在柵格地圖上設置障礙物標志；

智能決策模塊結合DSAS算法實現從環境的始發地出發，尋找一條能夠繞過障礙物的最優路徑以到達終點，實現實時路徑規劃；

所述視覺SLAM系統包括特征提取、跟蹤、建圖以及回環；

所述特征提取用于提取航拍影像中的特征點并計算描述子；

所述跟蹤通過計算當前幀與前一幀的特征點描述子之間的距離以完成特征匹配，再通過特征匹配恢復出當前幀相對與上一幀的位姿變化，其中，獲得上一幀位姿包括：

針孔相機的成像的數學表達式為：

其中Pi[Xi,Yi,Zi]為第i個地圖點的三維坐標，pi＝[ui,vi]為第i個地圖點在當前幀的投影的像素坐標，si為第pi對應的深度值，K為當前幀的相機內參，T當前幀的位姿，其中，T可以寫成李代數的形式：

在實際中由于噪聲的存在，Pi的真實投影后的像素坐標ui與理論值pi之間存在著誤差ei，把帶有噪聲的真實像素坐標記為觀測值，當誤差滿足高斯分布時：

便可以構建最小二乘問題，即：

記ei關于ξ的導數為雅克比J(ξ)，首先將世界坐標系下轉換到相機坐標系下為：

C_i＝exp(ξ^)P_i＝TP_i＝[X_i′，Y_i′，Z′_i]^T

相機模型可以表示為：

這里的[u′，v′]T即為像素坐標預測值的具體值，由si＝Z′消去si可以得到：

在式誤差項的公式中，將Ci作為中間變量，利用鏈式求導法則可以得出：

其中：

Ci關于ξ的偏導，相當于對李代數求導，可以構建擾動模型來完成，對ξ^左乘擾動量δξ，然后求TPi的增量對擾動項δξ的導數:

重新整理誤差項關于ξ的偏導可以得出：

雅克比矩陣前面的負號是由于推導過程中的誤差是由觀測值減預測值定義的，對式中Σ-1進行cholesky分解，即：

∑^-1＝LL^T＝L^TL

將上式帶入可得：

于是最小二乘法的公式便可以寫成：

當進行迭代優化時，這里為了簡單起見以高斯最優算法為例，針對誤差項進行泰勒展開：

其中J(ξ)為e(ξ)關于ξ的導數(雅克比)，可以化為：

對單一的二范數進行展開得：

求上式關于ξ的導數等于零可得：

即可得到增量方程：

由增量方程便可求解增量δξ，從而實現迭代優化；

所述建圖在當前幀與前幾幀中尋找更多的特征匹配，并通過三角測量恢復出更多的3D點，將當前幀與有共同觀測的關聯幀以及共同觀測的地圖點做局部的BA優化來獲取更為精確的相機位姿和3D地圖點坐標；

所述回環用來檢測相機是否回到之前去過的地方，并根據檢測到的回環添加新的約束來修正相應的相機位姿與3D地圖點坐標。