[發(fā)明專利]基于多傳感器融合的管道爬壁機(jī)器人航跡推算方法及系統(tǒng)

在審

• 申請(qǐng)?zhí)枺?/td>: 202211322414.7
• 申請(qǐng)日: 2022-10-27
• 公開(kāi)（公告）號(hào): CN115597606A
• 公開(kāi)（公告）日: 2023-01-13
• 發(fā)明（設(shè)計(jì)）人: 李琳;魏振曄;張鐵
• 申請(qǐng)（專利權(quán)）人: 華南理工大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G06F17/16
• 代理公司: 廣州市華學(xué)知識(shí)產(chǎn)權(quán)代理有限公司 44245
• 代理人: 梁睦宇

1.基于多傳感器融合的管道爬壁機(jī)器人航跡推算方法，其特征在于：包括以下步驟：

S1、將用于檢測(cè)每一運(yùn)動(dòng)時(shí)刻加速度的慣性測(cè)量單元和用于檢測(cè)兩次運(yùn)動(dòng)時(shí)刻之間相對(duì)位移的光學(xué)導(dǎo)航傳感器設(shè)置于管道爬壁機(jī)器人，且慣性測(cè)量單元和光學(xué)導(dǎo)航傳感器的數(shù)據(jù)以相同頻率采集數(shù)據(jù)；

S2、在EKF的算法框架中將慣性測(cè)量單元和光學(xué)導(dǎo)航傳感器采集的數(shù)據(jù)進(jìn)行融合；

S3、引入BPNN模型補(bǔ)償慣性測(cè)量單元由靜態(tài)漂移和動(dòng)態(tài)響應(yīng)差引起的誤差；

S4、再通過(guò)基于ANFIS的協(xié)方差匹配技術(shù)估計(jì)測(cè)量噪聲信息，減弱測(cè)量過(guò)程的不確定性給運(yùn)動(dòng)狀態(tài)估計(jì)帶來(lái)的噪聲干擾；

S5、以機(jī)器人起始點(diǎn)為坐標(biāo)原點(diǎn)，設(shè)置三維絕對(duì)坐標(biāo)系，并以原點(diǎn)在管道橫截面圓周的逆時(shí)針切線方向?yàn)閄軸正方向，機(jī)器人沿管道運(yùn)動(dòng)方向?yàn)閅軸正方向，垂直向上方向?yàn)閆軸正向；將管道爬壁機(jī)器人在二維虛擬平面估計(jì)的運(yùn)動(dòng)狀態(tài)通過(guò)幾何關(guān)系與數(shù)學(xué)運(yùn)算轉(zhuǎn)換成三維絕對(duì)坐標(biāo)系當(dāng)中的最優(yōu)運(yùn)動(dòng)狀態(tài)，得到機(jī)器人在管道絕對(duì)坐標(biāo)系中的真實(shí)位置p(k)、速度v(k)、加速度a(k)。

2.根據(jù)權(quán)利要求1所述的基于多傳感器融合的管道爬壁機(jī)器人航跡推算方法，其特征在于：步驟S2中，數(shù)據(jù)進(jìn)行融合的具體過(guò)程如下：

S2-1、將管道內(nèi)壁面展開(kāi)為二維虛擬平面，以管道爬壁機(jī)器人起始點(diǎn)為坐標(biāo)原點(diǎn)，設(shè)置二維虛擬坐標(biāo)系，并以機(jī)器人沿管道運(yùn)動(dòng)方向?yàn)閅軸正方向，沿管道圓周逆時(shí)針切線方向?yàn)閄軸正方向；

S2-2、令K時(shí)刻慣性測(cè)量單元采集的到加速度向量為a_IMU(k)，光學(xué)導(dǎo)航傳感器采集到的相對(duì)位移向量為d_opt(k)，并基于加速度向量和相對(duì)位移向量建立運(yùn)動(dòng)，對(duì)管道爬壁機(jī)器人在管道中的運(yùn)動(dòng)狀態(tài)建立數(shù)學(xué)模型：

x(k)＝Ax(k-1)+w(k-1)，

z(k)＝Hx(k)+v(k)，

其中，w(k-1)為過(guò)程噪聲，v(k)為測(cè)量噪聲，A為上一狀態(tài)到當(dāng)前狀態(tài)的轉(zhuǎn)換矩陣，H為當(dāng)前狀態(tài)到測(cè)量向量的轉(zhuǎn)換矩陣，x(k)為狀態(tài)向量，z(k)為測(cè)量向量；

S2-3、令為k時(shí)刻狀態(tài)向量x(k)的先驗(yàn)估計(jì)，P(k|k-1)為k時(shí)刻先驗(yàn)估計(jì)誤差協(xié)方差矩陣，得：

P(k|k-1)＝AP(k-1)A^T+Q(k-1)，

其中T為慣性測(cè)量單元和光學(xué)導(dǎo)航傳感器發(fā)送數(shù)據(jù)信號(hào)的時(shí)間周期，Q(k-1)為過(guò)程噪聲w(k-1)的協(xié)方差矩陣；

S2-4、利用K時(shí)刻的測(cè)量向量z(k)和測(cè)量噪聲協(xié)方差矩陣R(k)的后驗(yàn)估計(jì)將預(yù)測(cè)階段的估計(jì)值和P(k|k-1)分別更新為狀態(tài)向量x(k)的后驗(yàn)估計(jì)和后驗(yàn)估計(jì)誤差協(xié)方差矩陣P(k)，以進(jìn)行數(shù)據(jù)融合得：

P(k)＝[I-K(k)H]P(k|k-1)，

其中，K(k)為卡爾曼增益。

3.根據(jù)權(quán)利要求2所述的基于多傳感器融合的管道爬壁機(jī)器人航跡推算方法，其特征在于：步驟S3的BPNN模型的建立過(guò)程如下：

S3-1、把k-1時(shí)刻的最優(yōu)狀態(tài)估計(jì)的加速度向量提取出來(lái)，選擇k時(shí)刻測(cè)得的相對(duì)位移向量d_opt(k)，并將和d_opt(k)作為BPNN模型的輸入特征；

S3-2、基于輸入特征，選擇切線傳遞函數(shù)作為輸入層與第一隱含層、第一隱含層與第二隱含層之間的傳遞函數(shù)，選擇線性傳遞函數(shù)作為第二隱含層與輸出層之間的傳遞函數(shù)，建立第一隱含層節(jié)點(diǎn)為5、第二隱含層節(jié)點(diǎn)為2的雙隱含層BPNN模型。