[發明專利]一種基于GPS、5G以及視覺的石化巡檢機器人導航定位方法

有效

• 申請號: 202010771058.1
• 申請日: 2020-08-04
• 公開（公告）號: CN111735458B
• 公開（公告）日: 2020-11-24
• 發明（設計）人: 賴欣;張晨蕾;江紅;李奧華;李嘉禾;王森;王寶寶;談峻菡
• 申請（專利權）人: 西南石油大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01D21/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于GPS、5G以及視覺的石化巡檢機器人導航定位方法，其特征在于：

(1)GPS模塊(1)采集量測信息Z₁(k)以特定頻率發送到GPS判斷模塊(5)，5G模塊(2)采集量測信息Z₂(k)以特定頻率發送到5G判斷模塊(6)、視覺模塊(3)采集量測信息Z₃(k)以特定頻率發送到視覺判斷模塊(7)；

(2)里程計模塊(4)的輸出量以特定頻率發送到濾波器A(8)、濾波器B(9)和濾波器C(10)，石化巡檢機器人以里程計輸出導航參數的誤差量作為狀態量，結合狀態量構建里程計誤差模型m是粒子序號，且m＝1,2,3,…,M,M是粒子樣本總數，是k時刻粒子樣本中第m個粒子的狀態值，是k-1時刻粒子樣本中第m個粒子的狀態值，為先驗估計，即當前時刻狀態僅與上一時刻狀態有關；

(3)GPS判斷模塊(5)結合環境特征地圖(14)判定可用量測信息并發送到濾波器A(8)，5G判斷模塊(6)結合環境特征地圖(14)判定可用量測信息并發送到濾波器B(9)，視覺判斷模塊(7)結合環境特征地圖(14)判定可用量測信息并發送到濾波器C(10)，接收到信息的濾波器A(8)、濾波器B(9)和濾波器C(10)采用里程計輸出量與GPS模塊(1)、5G模塊(2)和視覺模塊(3)輸出的量測信息差值作為量測方程，構建量測模型并計算重要性權值：進行歸一化權值處理，得到：其中Z_j(k)中，當j＝1，2,3時分別表示GPS模塊(1)、5G模塊(2)、視覺模塊(3)采集的量測信息，中，當j＝1，2,3時分別表示濾波器A(8)、濾波器B(9)和濾波器C(10)中第m個粒子的重要性權值，中，當j＝1，2,3時分別表示濾波器A(8)、濾波器B(9)和濾波器C(10)中第m個粒子的歸一化重要性權值，為似然概率密度函數，即為石化巡檢機器人在狀態下觀察Z_j(k)的概率，p(Z_j(k))為邊緣概率，為重要性密度函數，中，當j＝1,2,3時分別表示濾波器A(8)、濾波器B(9)和濾波器C(10)在k時刻粒子樣本中第m個樣本值，且符合重要性密度函數

(4)濾波器A(8)、濾波器B(9)和濾波器C(10)利用并行隱式等權粒子濾波定位算法建立機器人誤差模型和量測模型獲得相應的局部狀態估計和局部協方差其中中，當j＝1，2，3時分別表示濾波器A(8)、濾波器B(9)、濾波器C(10)輸出的石化巡檢機器人的局部狀態估計；中，當j＝1，2，3時分別表示濾波器A(8)、濾波器B(9)、濾波器C(10)輸出的石化巡檢機器人的局部協方差；濾波器A(8)以特定頻率將局部狀態估計和局部協方差發送到分布式多傳感器融合模塊A(11)；濾波器B(9)以特定頻率將局部狀態估計和局部協方差發送到分布式多傳感器融合模塊A(11)和分布式多傳感器融合模塊B(12)；濾波器C(10)以特定頻率將局部狀態估計和局部協方差發送到分布式多傳感器融合模塊B(12)，

更新石化巡檢機器人的局部狀態估計：

更新石化巡檢機器人的局部協方差：

(5)由分布式多傳感器融合模塊A(11)接收來自濾波器A(8)的局部狀態估計局部協方差和來自濾波器B(9)的局部狀態估計局部協方差分布式多傳感器融合模塊A(11)利用最優信息融合準則得到組合狀態估計為：

組合協方差估計表達式為：

其中為最優權值矩陣,E＝(e₄，e₄)^T，e₄是4×4單位矩陣；將融合后的組合狀態估計X₁(k)和組合協方差P₁(k)發送到融合信息定位模塊(13)；

由分布式多傳感器融合模塊B(12)接收來自濾波器B(9)的局部狀態估計和局部協方差以及來自濾波器C(10)的局部狀態估計和局部協方差分布式多傳感器融合模塊B(12)利用最優信息融合準則得到組合狀態估計為：

組合協方差估計表達式為：

其中為最優權值矩陣，將融合后的組合狀態估計X₂(k)和組合協方差P₂(k)發送到融合信息定位模塊(13)；

(6)融合信息定位模塊(13)利用最優信息融合準則將接收到的組合狀態估計和組合協方差進行融合，決策出全局最佳狀態估計X以及全局協方差P，其中為最優權值矩陣，

2.基于權利要求1所述的導航定位方法，其特征在于，在步驟(3)中，采用調節α和β兩個參數對每個粒子進行更新：由于是粒子的確定性移動，經坐標轉換其中為雅可比矩陣行列式的絕對值，中，當j＝1，2，3時分別表示濾波器A(8)、濾波器B(9)和濾波器C(10)在k-1時刻輸出的局部協方差，ξ^m是從標準多元高斯分布中得出，S^m是一個隨機向量，ξ^m、S^m以及q(ξ)均為標準正態分布N(0,1)，N(0,1)是期望為0，標準差為1的概率分布。