CN115792947B 一種基于ekf的管道巡檢機器人激光雷達輔助實時定位方法 （南京理工大學）

(12)發(fā)明專利李勝李福馬寅瑞姜燁YuDing.PipeenvironmenCloud.《2022ICCSI》.2022,202-207.人激光雷達輔助實時定位方法，建立動力學模21.一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，其特征在于，包括以下步驟1、建立通風管道巡檢機器人系統(tǒng)動力學模型；步驟2、將機器人放置在通風管道中采集點云數(shù)據(jù)并人工標注，訓練激光雷達點云關于通風管道管壁的反射畸變模型人工神經(jīng)網(wǎng)絡；步驟3、在開始工作點初始化機器人狀態(tài)及協(xié)方差；步驟4、通過系統(tǒng)方程預測狀態(tài)矢量和協(xié)方差；步驟5、通過安裝于機器人側面的激光雷達采集點云數(shù)據(jù)，并基于反射畸變校正人工神經(jīng)網(wǎng)絡對于在通風管道環(huán)境下的畸變點云進行去畸變處理；步驟6、基于激光雷達點云強度與密度提取直線特征進行逆解算計算機器人朝向角與橫向偏移；步驟7、由測量值更新卡爾曼增益、機器人系統(tǒng)狀態(tài)及協(xié)方差。2.根據(jù)權利要求1所述的一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，其特征在于，步驟1中的建立通風管道巡檢機器人系統(tǒng)動力學模型，具體步驟如下：步驟1-1、建立管道機器人陀螺儀與編碼器的傳感器模型：分別為機器人在導航坐標系下沿x軸與v軸速度.v與θ分別為機器人在導航坐標系下編碼器輸出速度與航向角；步驟1-2、利用機器人陀螺儀與編碼器的傳感器模型建立機器人系統(tǒng)動力學模型為：別表示機器人在導航坐標系下k時刻的沿x軸方向坐標，沿y軸方向坐標，沿x軸方向速度，沿y軸速度及朝向角。3.根據(jù)權利要求1所述的一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，其特征在于，步驟2中的將機器人放置在通風管道中采集點云數(shù)據(jù)并人工標注，訓練激光雷達點云關于通風管道管壁的反射畸變模型人工神經(jīng)網(wǎng)絡，具體步驟如下：步驟2-1、將機器人放置在工作環(huán)境中，對于采集的點云數(shù)據(jù)的距離屬性根據(jù)人工測量真值進行人工標注修改后與點云強度與角度重新組合構建訓練集t;步驟2-2、構建多層感知器結構的全連接網(wǎng)絡M,具體為：3其中，向量x、y分別為網(wǎng)絡的輸入層和輸出層，x向量由點云的角度和強度組成，y向量活函數(shù)分別為tansig函數(shù)和purelin函數(shù)；步驟2-3、根據(jù)損失函數(shù)J計算網(wǎng)絡M輸出結果與真實值的誤差，迭代訓練網(wǎng)絡參數(shù)，具體為：4.根據(jù)權利要求1所述的一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，其特征在于，步驟3中的在開始工作點初始化機器人狀態(tài)及協(xié)方差，具體步驟如下：定義機器人系統(tǒng)動力學狀態(tài)與真實狀態(tài)的誤差狀態(tài)矢量為：δx、δy分別為機器人在導航坐標系下位置坐標誤差；δvxδv,為機器人在導航坐標系下移誤差；系統(tǒng)協(xié)方差由機器人裝配的編碼器及陀螺儀性能確定，T為矩陣轉置符號。5.根據(jù)權利要求1所述的一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，其特征在于，步驟4中的通過系統(tǒng)方程預測誤差狀態(tài)矢量和協(xié)方差，具體為：4o=[0√2Yoa02aδt√2β?o28t]T因子的自相關時間常數(shù)，o2a為對應的方差；β?為陀螺儀隨機漂移的自相關時間常數(shù)，o2為對應的方差。6.根據(jù)權利要求1所述的一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，其特征在于，步驟5中的激光雷達點云距離范圍為[0.03m,0.3m,角度范圍為,采集頻率為30Hz。7.根據(jù)權利要求1所述的一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，其特征在于，步驟6中基于激光雷達點云強度與密度提取直線特征進行解算計算機器人朝步驟6-1、對于步驟3中經(jīng)去畸變處理的點云，選取具有最大反射強度的點p;步驟6-3、將此屬性與距離閾值∈相比較，如果在p點周圍至少存在minpts個點，則將這些點聚類為一類，具體的，參數(shù)∈,minpts分別設置為0.02m和10;步驟6-4、在剩余點中重復步驟4-1至4-3直到所有的點都被判別過；步驟6-7、計算在該類中其余點關于該直線的距離的誤差函數(shù)，具體為：步驟6-8、重新隨機選取兩個點，重復步驟4-5至4-7直到選取完成所有點；步驟6-9、選取具有最小誤差函數(shù)值的一組點，計算直線模型參數(shù)作為所提取的直線特征mbest,best;步驟6-10、對線特征進行解算計算機器人朝向角與橫向偏移，具體為：8.根據(jù)權利要求1所述的一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，其特征在于，步驟7中的由測量值更新卡爾曼增益、機器人系統(tǒng)狀態(tài)及協(xié)方差，計算機器人位姿具體為：5其中，δZ為由步驟6中激光雷達解算出的位姿與機器人動力學模型估計的位姿之差，yLidar和yRISS分別為激光雷達解算的在導航坐標系下機器人的y軸坐標與機器人系統(tǒng)動力學模型解算的在導航坐標系下機器人的y軸坐標，θLi導航坐標系下機器人的朝向角與機器人系統(tǒng)動力學模型解算的在導航坐標系下機器人的為設置為高斯白噪聲的測量誤差向量；步驟7-2、由系統(tǒng)誤差測量值更新卡爾曼增益、機器人系統(tǒng)狀態(tài)及協(xié)方差。6技術領域[0001]本發(fā)明屬于管道機器人定位技術領域，特別是涉及一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法。背景技術[0002]具有增強清潔和過濾的通風系統(tǒng)已成為預防疫情的重要途徑，通風管道的消毒和清潔是保證空氣質(zhì)量的關鍵，而管道機器人的應用可以代替人工進行消毒和清潔作業(yè)，可以極大的減少勞動力成本和潛在的生物感染風險。對于自主化的管道機器人，定位是必不可少的功能，目前管道機器人現(xiàn)有定位技術分為兩類，一類利用基于預先埋設于管內(nèi)的磁電標記點進行定位，然而這種方法必須在鋪設管道時人工布置信號源點，極大的增加了成本。另一類不依賴于信號源點的定位技術通常采用里程計和慣性測量單元組合進行定位，此類方法對于傳感器性能要求較高，狹小的通風管道對于傳感器的選型有較大的限制，而且由于通風管道內(nèi)環(huán)境復雜，極易造成車輪滑動而產(chǎn)生累積誤差，同時低成本的慣性測量單元對于角度測量檢測誤差較大。因此急需一種適用于通風管道巡檢機器人的，能夠不依賴于外部定位數(shù)據(jù)源的定位方法。發(fā)明內(nèi)容[0003]本發(fā)明目的在于解決管道機器人在復雜密閉的通風管道自主工作中的定位易產(chǎn)生累積誤差的問題，實現(xiàn)對管壁的點云反射去畸變處理與特征提取，并通過擴展卡爾曼濾波器對機器人系統(tǒng)動力學模型進行融合以對機器人進行更為準確的實時定位。[0004]為了實現(xiàn)本發(fā)明目的，本發(fā)明公開了一種基于擴展卡爾曼濾波EKF(ExtendedKalmanFilter)的通風管道巡檢機器人激光雷達輔助實時定位方法，包括以下步驟：[0006]步驟2、將機器人放置在通風管道中采集點云數(shù)據(jù)并人工標注，訓練激光雷達點云關于通風管道管壁的反射畸變模型人工神經(jīng)網(wǎng)絡；[0009]步驟5、通過安裝于機器人側面的激光雷達采集點云數(shù)據(jù)，并基于反射畸變校正人工神經(jīng)網(wǎng)絡對于在通風管道環(huán)境下的畸變點云進行去畸變處理；[0010]步驟6、基于激光雷達點云強度與密度提取直線特征進行逆解算計算機器人朝向角與橫向偏移；[0012]進一步地，步驟1中的建立通風管道巡檢機器人系統(tǒng)動力學模型.具體步驟如下：[0013]步驟1-1、建立管道機器人陀螺儀與編碼器的傳感器模型：7[0015]其中，①為管道機器人角速度，①2為機器人陀螺儀輸出vx與v,分別為機器人在導航坐標系下沿x軸與y軸速度，v與θ分別為機器人在導航坐標系下編碼器輸出速度與航向角；[0016]步驟1-2、利用機器人陀螺儀與編碼器的傳感器模型建立機器人系統(tǒng)動力學模型分別表示機器人在導航坐標系下k時刻的沿x軸方向坐標，沿y軸方向坐標，沿x軸方向速度，沿y軸速度及朝向角。[0019]進一步地，步驟2中的將機器人放置在通風管道中采集點云數(shù)據(jù)并人工標注，訓練激光雷達點云關于通風管道管壁的反射畸變模型人工神經(jīng)網(wǎng)絡，具體步驟如下：[0020]步驟2-1、將機器人放置在工作環(huán)境中，對于采集的點云數(shù)據(jù)的距離屬性根據(jù)人工測量真值進行人工標注修改后與點云強度與角度重新組合構建訓練集t;[0023]其中，向量x、y分別為網(wǎng)絡的輸入層和輸出層，x向量由點云的角度和強度組成，y中激活函數(shù)分別為tansig函數(shù)和purelin函數(shù)；[0024]步驟2-3:根據(jù)損失函數(shù)J計算網(wǎng)絡M輸出結果與真實值的誤差，迭代訓練網(wǎng)絡參[0027]進一步地，步驟3中的在開始工作點初始化機器人狀態(tài)及協(xié)方差，具體步驟如下：[0028]定義機器人系統(tǒng)動力學狀態(tài)與真實狀態(tài)的誤差狀態(tài)矢量為：8德漂移誤差；系統(tǒng)協(xié)方差由機器人裝配的編碼器及陀螺儀性能確定，T為矩陣轉置符號。刻度因子的自相關時間常數(shù)，o2a為對應的方差；β?為陀螺儀隨機漂移的自相關時間常數(shù)，o2為對應的方差。[0043]進一步地，步驟5中的激光雷達點云距離范圍為[0.03m,0.3m],角度范圍為,采集頻率為30Hz。[0044]進一步地，步驟6中基于激光雷達點云強度與密度提取直線特征進行解算計算機[0045]步驟6-1、對于步驟3中經(jīng)去畸變處理的點云，選取具有最大反射強度的點p;[0050]步驟6-4、在剩余點中重復步驟4-1至4-3直到所有的點都被判別過；9線特征m6est,best;[0065]其中，8Z為由步驟6中激光雷達解算出的位姿與機器人動力學模型估計的位姿之在導航坐標系下機器人的朝向角與機器人系統(tǒng)動力學模型解算的在導航坐標系下機器人云因材料反射的畸變現(xiàn)象，同時本發(fā)明的特征提取方法顯著減少了運行時間與資源消耗，可以實現(xiàn)在嵌入式機器人上的應用；3)本發(fā)明能夠在復雜的管道環(huán)境背景下取得良好效附圖說明[0070]此處所說明的附圖用來提供對本發(fā)明的進一步理解，構成本申請的一部分，本發(fā)明的示意性實施例及其說明用于解釋本發(fā)明，并不構成對本發(fā)明的不當限定。在附圖中：[0071]圖1為一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法的原理圖。具體實施方式[0072]下面將結合本發(fā)明實施例中的附圖，對本發(fā)明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施例僅是本發(fā)明一部分實施例，而不是全部的實施例；基于本發(fā)明中的實施例，本領域普通技術人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其他實[0073]如圖1所示，一種基于EKF的管道巡檢機器人激光雷達輔助實時定位方法，包括以[0075]步驟2、將機器人放置在通風管道中采集點云數(shù)據(jù)并人工標注，訓練激光雷達點云關于通風管道管壁的反射畸變模型人工神經(jīng)網(wǎng)絡；[0078]步驟5、通過安裝于機器人側面的激光雷達采集點云數(shù)據(jù)，并基于反射畸變校正人工神經(jīng)網(wǎng)絡對于在通風管道環(huán)境下的畸變點云進行去畸變處理；[0079]步驟6、基于激光雷達點云強度與密度提取直線特征進行逆解算計算機器人朝向角與橫向偏移；[0081]具體地，在本實施例中，步驟1中的建立通風管道巡檢機器人系統(tǒng)動力學模型，具體步驟如下：[0082]步驟1-1、建立管道機器人陀螺儀與編碼器的傳感器模型：[0084]其中，①為管道機器人角速度，①2為機器人陀螺儀輸出角速度，b?為陀螺儀零偏，vx與v,分別為機器人在導航坐標系下沿x軸與y軸速度，v與θ分別為機器人在導航坐標系下編碼器輸出速度與航向角；[0085]步驟1-2、利用機器人陀螺儀與編碼器的傳感器模型建立機器人系統(tǒng)動力學模型測量真值進行人工標注修改后與點云強度與角度重新組合[0093]步驟2-3、根據(jù)損失函數(shù)J計算網(wǎng)絡M輸出結果與真實值的誤差，迭代訓練網(wǎng)絡參儀隨機漂移的自相關時間常數(shù)，σ2為對應的方差。[0108]具體地，在本實施例中，步驟5