[發明專利]基于機器人平臺的隧道激光點云采集方法及機器人系統

有效

• 申請號: 202011501913.3
• 申請日: 2020-12-17
• 公開（公告）號: CN112729252B
• 公開（公告）日: 2022-05-20
• 發明（設計）人: 余建偉;李清泉;陳智鵬;廖江海;熊智敏;毛慶洲;熊永鋼
• 申請（專利權）人: 深圳市數字城市工程研究中心;深圳大學
• 主分類號: G01C11/04
• 分類號: G01C11/04;G01C21/20;G01C21/16
• 代理公司: 深圳市君勝知識產權代理事務所(普通合伙) 44268
• 代理人: 劉文求;朱陽波

1.一種基于機器人平臺的隧道激光點云采集方法，其特征在于，包括：

獲取當前第一位置、當前第一姿態及隧道內預設控制點，確定當前第一激光點云；

對比所述預設控制點的觀測坐標與多個預先實測得到的控制點的實際坐標，確定觀測所述預設控制點的觀測誤差；其中，機器人上配置有標定好的雙目立體相機，通過雙目立體相機拍攝隧道中預先設置的控制點圖像，通過對圖像進行分析，能夠得到預設控制點的觀測坐標；預設控制點的實際坐標是預先測量好的標準坐標；

根據所述觀測誤差對所述當前第一位置和所述當前第一姿態進行糾正，得到當前第二位置和當前第二姿態；其中包括：獲取行進過程中的位置增量，根據所述位置增量確定所述當前第一位置和所述當前第一姿態的慣性殘差；將所述慣性殘差和所述觀測誤差作為擴展卡爾曼濾波模型的輸入，輸出對所述當前第一位置和所述當前第一姿態的誤差修正量；根據所述誤差修正量糾正所述當前第一位置和所述當前第一姿態，分別得到所述當前第二位置和所述當前第二姿態；根據所有的當前第二位置和所有的當前第二姿態確定糾正后的第一行進軌跡；通過RIS平滑算法對所述第一行進軌跡進行反向平滑處理，之后進行內插，得到第二行進軌跡；

通過地圖匹配算法，建立所述當前第二位置與預設路線的匹配關系；

根據所述匹配關系確定所述當前第二位置和所述當前第二姿態與所述預設路線之間的偏差參數；其中，根據當前第二位置和當前第二姿態確定當前坐標點，將該坐標點與預設路線上對應位置點的坐標進行比對，確定當前行進路徑的角度偏差，通過機器人系統坐標系和預設路線坐標系之間的關系調整機器人的行進方向；

按照所述偏差參數調整行進過程中的行進參數，得到導航路線，其中，通過偏差參數對機器人的位置、姿態和速度參數進行修正，確定機器人的所述導航路線，為機器人在隧道中行走提供依據；

根據所述當前第二位置、所述當前第二姿態和所述導航路線，確定隧道三維點云；其中包括：

對每個對應的所述當前第一激光點云、所述當前第二位置和所述當前第二姿態分別進行坐標系轉換，在測圖坐標系中融合解算得到當前第二激光點云；

在測圖坐標系中融合解算的步驟如下：

當前第一激光點云在激光器坐標系即s系下的坐標表示為：

n^s＝[cosαsinβ，cosαsinβ,sinα]^T，x^s＝p·n^s

其中，x^s是當前第一激光點云在s系下的坐標，α是激光器發射水平角，β是激光器發射豎直角，p為激光測距值，n^s為激光射線的方向向量；

激光器坐標系即s系轉換到載體坐標系即b系，轉換公式如下：

其中，x^b是當前第一激光點云從s系轉換到b系下的坐標，為s系原點到b系原點的三維平移分量，也稱桿臂值，是s系相對b系的選擇矩陣；

當通過雙目立體相機采集到控制點后，則將載體坐標系的觀測值解算到測圖坐標系即導航坐標系中，其轉換公式如下：

其中，xⁿ是當前第一激光點云從b系轉換到導航坐標系下的坐標，為機器人當前第二位置的坐標信息，為b系到n系的旋轉矩陣，旋轉矩陣按照當前第二位置坐標向量中的三個歐拉角θx,θy,θz z-x-y旋轉順序左乘得到，其轉換公式為：

通過將激光器獲取的當前第一激光點云與當前第二位置和當前第二姿態通過坐標系轉換，將激光器獲取的當前第一激光點云轉換到載體坐標系再到導航坐標系中，推算出激光器獲取的隧道云數據的精確坐標；

其中對于控制點稀疏或者前期無控制點隧道，進行分段采集計算，其中，機器人系統啟動時先靜止一段時間，慣性導航單元的狀態收斂，從而測量出機器人系統的零偏誤差，然后控制激光器開始掃描，隨后停止掃描，機器人行駛一段距離后再次停下，經過一段時間靜止后掃描儀再繼續掃描，以此往復，獲取每段路程對應的多段當前第一激光點云；

當分段采集計算時，所述根據所述當前第二位置、所述當前第二姿態和導航路線，確定隧道三維點云還包括：

當存在多段所述第二行進軌跡時，分別對每段所述第二行進軌跡中的所述當前第二位置、所述當前第二姿態和所述當前第一激光點云進行坐標系轉換，在測圖坐標系中解算得到多段所述第二行進軌跡對應的所述當前第二激光點云；其中，將各個掃描點的坐標統一到載體坐標系中；

將多段所述第二行進軌跡中的所述當前第二激光點云根據所述第二行進軌跡分別進行初步拼接，得到隧道的多段初始激光點云；

提取多段所述初始激光點云中重合的點云進行配準；

對配準后的多段所述初始激光點云進行拼接，得到完整的隧道三維點云。