[發(fā)明專利]一種基于拐彎角的小徑管道機(jī)器人高精度定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202011633342.9
• 申請(qǐng)日: 2020-12-31
• 公開（公告）號(hào): CN112628524B
• 公開（公告）日: 2023-03-24
• 發(fā)明（設(shè)計(jì)）人: 安文斗;夏銘;熊荊;張中華;李愛冉
• 申請(qǐng)（專利權(quán)）人: 重慶安全技術(shù)職業(yè)學(xué)院
• 主分類號(hào): F16L55/32
• 分類號(hào): F16L55/32;F16L55/40;G01C21/16;G01C21/18;G01C21/20;G01C25/00;F16L101/30
• 代理公司: 重慶中之信知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 50213
• 代理人: 鄧鋒

1.一種基于拐彎角的小徑管道機(jī)器人高精度定位方法，其特征在于，包括如下步驟：

S11、用帶有慣性測(cè)量單元和若干里程儀的小車對(duì)管道進(jìn)行檢測(cè)；

S12、分別運(yùn)用慣性測(cè)量單元數(shù)據(jù)和里程儀數(shù)據(jù)實(shí)現(xiàn)對(duì)管道拐彎角的解算；

采用捷聯(lián)慣性導(dǎo)航算法計(jì)算出小徑管道檢測(cè)機(jī)器人在管道內(nèi)運(yùn)動(dòng)的姿態(tài)、速度和位置信息；

S13、結(jié)合里程儀檢測(cè)信息和管道非完整性約束對(duì)拐彎角檢測(cè)信息進(jìn)行位置、速度和姿態(tài)角誤差計(jì)算；

S14、根據(jù)初始位置計(jì)算管道三維位置信息；并借助長(zhǎng)度已知的直管道段長(zhǎng)度信息對(duì)位置信息進(jìn)行修正；

S15、采用Kalman濾波估計(jì)方法進(jìn)行誤差補(bǔ)償；

S16、數(shù)據(jù)平滑處理；

S17、確定管道缺陷位置。

步驟S13中所述誤差計(jì)算的方式包括：根據(jù)管道拐彎角檢測(cè)結(jié)果，將由里程儀拐彎角檢測(cè)在每個(gè)彎管段解算出來的方向角A_PB和俯仰角P_PB與對(duì)應(yīng)彎管道段由捷聯(lián)慣性導(dǎo)航算法計(jì)算出的方向角A_SINS和俯仰角P_SINS分別作差，得出整個(gè)彎管道段定位的方向角和俯仰角誤差；

由捷聯(lián)慣性導(dǎo)航算法計(jì)算出來的三維速度V_e,SINS，V_n,SINS和V_u,SINS與里程儀測(cè)量的軸向速度v_odo，以及管道測(cè)量裝置在管道內(nèi)橫向和縱向的非完整性約束提供的零速經(jīng)過坐標(biāo)變換再作差可得出速度誤差。

2.如權(quán)利要求1所述一種基于拐彎角的小徑管道機(jī)器人高精度定位方法，其特征在于：步驟S13中，方向角和俯仰角誤差計(jì)算的具體公式為:

其中，δx表示系統(tǒng)狀態(tài)變量，由三維位置誤差三維速度誤差三維姿態(tài)角誤差及3個(gè)陀螺儀漂移誤差和3個(gè)加速度計(jì)零偏誤差δfⁿ組成；[δη_P δη_A]^T分別表示俯仰角和方位角量測(cè)噪聲；系統(tǒng)觀測(cè)矩陣表示為：

捷聯(lián)慣性導(dǎo)航算法計(jì)算出來的三維速度V_e,SINS，V_n,SINS和V_u,SINS與測(cè)量的軸向速度v_odo，以及管道測(cè)量裝置在管道內(nèi)橫向和縱向的非完整性約束提供的零速經(jīng)過坐標(biāo)變換再作差可得出速度誤差；具體計(jì)算公式為

[δη_v,eδη_v,nδη_v,u]^T表示量測(cè)噪聲，系統(tǒng)從載體坐標(biāo)b系到導(dǎo)航坐標(biāo)n姿態(tài)變換矩陣，則系統(tǒng)觀測(cè)矩陣H₂表示為：

H₂＝[0_3×3 I_3×3 0_3×3 0_3×3]。

3.如權(quán)利要求2所述一種基于拐彎角的小徑管道機(jī)器人高精度定位方法，其特征在于，步驟S14中的修正方式為：已知的直管道段的長(zhǎng)度轉(zhuǎn)換到導(dǎo)航坐標(biāo)系，具體公式為：其中，為測(cè)量噪聲；表示第j段管道段長(zhǎng)度l_j在導(dǎo)航坐標(biāo)系n各軸分量，Z為觀測(cè)量；由捷聯(lián)慣性導(dǎo)航算法解算得出的管道段長(zhǎng)度為：

得出位置誤差：

系統(tǒng)觀測(cè)矩陣為：

H₃＝[I_3×3 0_3×3 0_3×3 0_3×3]。

4.如權(quán)利要求3所述一種基于拐彎角的小徑管道機(jī)器人高精度定位方法，其特征在于，步驟15中誤差補(bǔ)償?shù)木唧w方式為：將得出的被檢測(cè)管道在彎管道段的俯仰角、方向角誤差及三維連續(xù)速度誤差和直管道段的三維離散位置誤差為觀測(cè)量，采用Kalman濾波估計(jì)方法估計(jì)并補(bǔ)償慣性測(cè)量單元誤差和捷聯(lián)慣性導(dǎo)航算法誤差。

5.如權(quán)利要求4所述一種基于拐彎角的小徑管道機(jī)器人高精度定位方法，其特征在于，以狀態(tài)變量和協(xié)方差陣為基礎(chǔ)，通過反向的方式采用數(shù)據(jù)平滑處理技術(shù)再次對(duì)慣性測(cè)量單元誤差和捷聯(lián)慣性導(dǎo)航算法誤差進(jìn)行離線估計(jì)和補(bǔ)償。