[發(fā)明專利]水下結(jié)構(gòu)檢測(cè)機(jī)器人實(shí)時(shí)導(dǎo)航系統(tǒng)及方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201410413791.0
• 申請(qǐng)日: 2014-08-20
• 公開(kāi)（公告）號(hào): CN104197927B
• 公開(kāi)（公告）日: 2017-06-23
• 發(fā)明（設(shè)計(jì)）人: 曾慶軍;張明;眭翔;黃巧亮
• 申請(qǐng)（專利權(quán)）人: 江蘇科技大學(xué)
• 主分類號(hào): G01C21/00
• 分類號(hào): G01C21/00;G01C21/20
• 代理公司: 南京經(jīng)緯專利商標(biāo)代理有限公司32200
• 代理人: 樓高潮

1.一種水下結(jié)構(gòu)檢測(cè)機(jī)器人實(shí)時(shí)導(dǎo)航系統(tǒng)的導(dǎo)航方法，水下結(jié)構(gòu)檢測(cè)機(jī)器人實(shí)時(shí)導(dǎo)航系統(tǒng)包括磁羅盤(1)、陀螺儀(2)、加速度計(jì)(3)、深度計(jì)(4)、導(dǎo)航微處理器(5)，所述磁羅盤(1)、陀螺儀(2)、加速度計(jì)(3)和深度計(jì)(4)分別采集水下機(jī)器人磁場(chǎng)強(qiáng)度、角速度、線速度和下潛深度數(shù)據(jù)，傳輸至導(dǎo)航微處理器(5)；所述導(dǎo)航微處理器(5)對(duì)磁羅盤(1)、陀螺儀(2)、加速度計(jì)(3)、深度計(jì)(4)采集的數(shù)據(jù)進(jìn)行A/D轉(zhuǎn)換；所述導(dǎo)航微處理器(5)結(jié)合四元數(shù)梯度下降法、互補(bǔ)濾波、卡爾曼算法對(duì)磁羅盤(1)、陀螺儀(2)、加速度計(jì)(3)采集的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，獲取姿態(tài)矩陣和姿態(tài)角；所述導(dǎo)航微處理器(5)對(duì)陀螺儀(2)、加速度計(jì)(3)采集的數(shù)據(jù)采用帶有旋轉(zhuǎn)補(bǔ)償?shù)娜A迎風(fēng)算法求解速度和位置；所述導(dǎo)航微處理器(5)對(duì)深度計(jì)采用滑動(dòng)平均濾波求取水下機(jī)器人下潛深度，得到組合導(dǎo)航信息；其特征在于，水下結(jié)構(gòu)檢測(cè)機(jī)器人實(shí)時(shí)導(dǎo)航系統(tǒng)的導(dǎo)航方法，以四元數(shù)和角誤差為濾波估計(jì)對(duì)象，首先進(jìn)行互補(bǔ)濾波，對(duì)陀螺儀測(cè)量值作初步補(bǔ)償；其次運(yùn)用梯度下降法更新四元數(shù)，有效降低姿態(tài)角漂移；再進(jìn)行卡爾曼濾波，采用矩陣中的UD分解方法，避免濾波器發(fā)散，得到水下機(jī)器人姿態(tài)矩陣和姿態(tài)角；然后對(duì)加速度計(jì)和陀螺儀數(shù)據(jù)運(yùn)用三階迎風(fēng)算法求解速度和位置；同時(shí)，對(duì)深度計(jì)采集數(shù)據(jù)進(jìn)行滑動(dòng)平均濾波，得到水下機(jī)器人深度值，水下機(jī)器人的垂向位置以此為準(zhǔn)；該方法包括以下步驟：

1)姿態(tài)、速度、位置解算，過(guò)程如下：

⑴水下機(jī)器人導(dǎo)航系統(tǒng)采用的離散卡爾曼方程為：

X_k＝Φ_k,k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

式中，k表示系統(tǒng)離散采樣時(shí)間點(diǎn)，k≥0；X_k為k時(shí)刻的系統(tǒng)估計(jì)狀態(tài)量；Φ_k,k-1為X_k-1到X_k的一步轉(zhuǎn)移矩陣；W_k-1為系統(tǒng)激勵(lì)高斯白噪聲序列，均值為E[w(k)]＝0，方差為E[w(k)w(k)^T]＝Q_k，Q_k即系統(tǒng)噪聲方差；Z_k為k時(shí)刻的傳感器量測(cè)值；H_k為量測(cè)矩陣，反應(yīng)量測(cè)量和估計(jì)量之間的數(shù)學(xué)關(guān)系；V_k為量測(cè)值高斯白噪聲序列，均值為E[v(k)]＝0，方差為E[v(k)v(k)^T]＝R_k，R_k即測(cè)量噪聲方差；W_k、V_k互不相關(guān)；

⑵確定參數(shù)初始狀態(tài)，包括：

當(dāng)?shù)刂亓铀俣萭，系統(tǒng)采樣時(shí)間t，角速度誤差Δω₁、Δω₂、Δω₃，互補(bǔ)濾波參數(shù)k_p、k_i，被估計(jì)狀態(tài)量初值估計(jì)均方誤差矩陣初值P_0|0；其中k_p、k_i取值規(guī)則為：k_p+k_i＝1，0.95＜k_p＜1，通過(guò)多次試驗(yàn)確定最優(yōu)值；P_0|0按如下取值：設(shè)定導(dǎo)航坐標(biāo)系為東北天坐標(biāo)系，記為n系；設(shè)定機(jī)體坐標(biāo)系為b系，初始時(shí)刻與導(dǎo)航坐標(biāo)系重合，n系和b系均為三軸正交坐標(biāo)系；四元數(shù)表示坐標(biāo)旋轉(zhuǎn)關(guān)系Q＝[q₀,q₁,q₂,q₃]，其中q₀為旋轉(zhuǎn)標(biāo)量，q₁、q₂、q₃為坐標(biāo)旋轉(zhuǎn)矢量，用四元數(shù)表述n系到b系轉(zhuǎn)換關(guān)系表示為：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_0)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

而b系到n系為r_ij(1≤i,j≤3)表示矩陣中各元素值；

⑶利用陀螺儀和磁羅盤、加速度計(jì)的頻域互補(bǔ)特性校正角速度矢量，過(guò)程如下：

設(shè)磁羅盤測(cè)量值為m_b，加速度計(jì)測(cè)量值為a，陀螺儀測(cè)量值為ω，m_b、a、ω均為3×1列矩陣；

首先把磁場(chǎng)量由機(jī)體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系并計(jì)算水平和垂直方向磁場(chǎng)：by＝0、bz＝m_nz；

然后把磁場(chǎng)轉(zhuǎn)換到機(jī)體坐標(biāo)系：并利用第三列估計(jì)重力加速度矢量：

再計(jì)算加速度和磁場(chǎng)補(bǔ)償誤差：

式中，“×”表示向量乘法；

得到累積誤差為：errInt＝KiΣerr*t；

最后得到校正之后的角速度矢量：ω_c＝ω+k_p*err+errInt

其中，K_i、K_p是互補(bǔ)濾波參數(shù)；

⑷利用梯度下降法更新四元數(shù)：

由得到新四元數(shù)q₀、q₁、q₂、q₃，其中，μ是梯度下降的步長(zhǎng)值，由確定；d為傳感器基于機(jī)體坐標(biāo)系的測(cè)量值，s為傳感器基于導(dǎo)航坐標(biāo)系的實(shí)際值，“*”表示伴隨矩陣，為梯度乘法，為四元數(shù)乘法；

⑸基于UD分解的卡爾曼濾波過(guò)程依次為：

狀態(tài)一步預(yù)測(cè)：

UD分解：H_kP_k/k-1H_k^T+R＝U∧D；

一步預(yù)測(cè)均方誤差：P_k/k-1＝Φ_k,k-1P_k-1/k-1Φ_k/k-1^T+Q_k-1；

計(jì)算濾波增益：K_k＝P_k/k-1H_k^T(H_kP_k/k-1H_k^T+R)^-1＝P_k/k-1H_k^T(UDU^T)^-1；

計(jì)算新息，新息由最新量測(cè)值計(jì)算獲得：

狀態(tài)估值計(jì)算：

估計(jì)均方誤差：P_k/k＝(I-K_kH_k)P_k/k-1；

⑹運(yùn)用三階迎風(fēng)格式求解速度和位置，并進(jìn)行旋轉(zhuǎn)補(bǔ)償，按如下過(guò)程計(jì)算：

設(shè)加速度計(jì)在k時(shí)刻、k-1時(shí)刻、k-2時(shí)刻的測(cè)量值分別為a_k、a_k-1、a_k-2，陀螺儀在k時(shí)刻、k-1時(shí)刻、k-2時(shí)刻的測(cè)量值分別為ω_k、ω_k-1、ω_k-2，令T＝2t，則T_m、T_m-1、T_m-2時(shí)刻的速度分別設(shè)為v_m、v_m-1、v_m-2，T_m、T_m-1時(shí)刻的位置分別為P_m、P_m-1；

首先計(jì)算兩個(gè)采樣時(shí)刻的速度增量和角增量：

速度增量為

角增量為

然后采用三階迎風(fēng)格式求解速度和位置：

速度為

位置為

式中，即為旋轉(zhuǎn)補(bǔ)償部分，“×”表示向量乘法；

2)深度計(jì)解算

對(duì)深度計(jì)采用等權(quán)端點(diǎn)平滑作滑動(dòng)平均濾波，計(jì)算水下機(jī)器人下潛深度：

$depth=\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}{d}_i$

不斷逐個(gè)滑動(dòng)地去取m個(gè)相鄰數(shù)據(jù)作算數(shù)平均計(jì)算，式中，d_i是深度計(jì)采集的第i個(gè)數(shù)據(jù)，depth為濾波后的深度值。