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