水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)及運動控制方法
【專利摘要】本發(fā)明公開了一種水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)及運動控制方法。水面系統(tǒng)包含水面控制臺，水面通信收發(fā)器，臍帶纜，水下系統(tǒng)包含水下通信收發(fā)器，供電單元，嵌入式微控制器，動力推進單元，視覺照明單元，運動切換單元，安全保護單元，傳感器單元。各功能模塊采用模塊化設(shè)計，方便安裝與拆卸，水面控制臺具有緊急拍停功能，配備了無線連接的平板電腦，可以實現(xiàn)移動化監(jiān)控；通信方式多樣，數(shù)據(jù)量豐富；運動切換單元支持浮游和爬行兩種運動方式切換；擁有數(shù)據(jù)存儲、電壓電流檢測等可靠的安全保護。本發(fā)明基于廣義預(yù)測PID控制算法的運動控制方法，有效減少系統(tǒng)能源消耗，降低元器件負載，更加方便和穩(wěn)定地控制機器人。
【專利說明】水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)及運動控制方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種水下結(jié)構(gòu)物檢測機器人，特別是一種水下結(jié)構(gòu)物檢測機器人的控制系統(tǒng)和運動控制方法，屬于機器人【技術(shù)領(lǐng)域】。

【背景技術(shù)】
[0002]國外很早就開始了對水下結(jié)構(gòu)物檢測機器人的研究，而且也取得了一定的研究成果。如美國生產(chǎn)的大型水下機器人，SUPER SCORP1,SUPER PHANTOM、TRITON等都成功地用于海上石油鉆井平臺設(shè)備的安裝、檢測、輔助及維護水下生產(chǎn)和常規(guī)作業(yè)。法國國家海洋開發(fā)中心與一家公司合作，共同建造“埃里特”聲學(xué)遙控潛水器。用于水下鉆井機檢查、海底油機設(shè)備安裝、油管鋪設(shè)、錨纜加固等復(fù)雜作業(yè)。英國從上個世紀九十年代初開始，開展了用于海洋石油開發(fā)水下檢測的自動遠程操縱系統(tǒng)項目研究開發(fā)。該系統(tǒng)布于近海現(xiàn)場，用于檢測Mobile公司的Beryl Bravo平臺。挪威開發(fā)的REMO是一種基于遙控潛水器的水下檢測機器人，用于復(fù)雜鋼管節(jié)點焊縫的清潔與檢測，近年來，隨著用于消除焊縫裂紋的機器人系統(tǒng)的發(fā)展，REMO系統(tǒng)的性能也已經(jīng)大大發(fā)展。
[0003]在我國對于水下結(jié)構(gòu)物檢測機器人的研究還只能說是起步不久，相關(guān)的經(jīng)驗不是很豐富，國產(chǎn)水下機器人(ROV)主要集中在上海交通大學(xué)、中國科學(xué)院沈陽自動化研究所等幾家單位，從事應(yīng)用型ROV產(chǎn)品開發(fā)的公司較少，而且探測技術(shù)、工藝水平、導(dǎo)航與定位、復(fù)雜環(huán)境中的運動控制等方面與國外存在較大差距。
[0004]隨著計算機技術(shù)、GPS定位技術(shù)及通訊傳感技術(shù)的快速發(fā)展，遠程智能水下結(jié)構(gòu)物檢測機器人在復(fù)雜水下環(huán)境中的觀察能力、頂流作業(yè)能力、高精度的運動控制及定位能力等均需要進一步提高，同時通過改善水面支持系統(tǒng)與水下觀察作業(yè)系統(tǒng)之間的人機交互界面，加大數(shù)據(jù)處理容量，全面提高水下結(jié)構(gòu)物檢測機器人操作控制水平和操作性能是發(fā)展趨勢之一。申請?zhí)枮?01110373831.X，名稱為“一種復(fù)合吸附船體清刷機器人”，其只能實現(xiàn)水下行走功能，功能單一；申請?zhí)枮?00310105200，名稱為“一種分布式水下機器人控制系統(tǒng)”，它僅僅采用了一路RS-485網(wǎng)絡(luò)通信，可靠性低，數(shù)據(jù)傳輸量也少；申請?zhí)枮?01310357116.6名稱為“一種淺水用小型水下機器人甲板的控制裝置”，沒有緊急拍停按鈕，遇到特殊情況處置不方便，也沒有配備平板電腦，不能實現(xiàn)移動監(jiān)控機器人，不方便其他人員觀察機器人作業(yè)。


【發(fā)明內(nèi)容】

[0005]本發(fā)明的目的在于提供一種水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)及運動控制方法，解決現(xiàn)有技術(shù)缺陷，提供一種能夠傳輸多種信號，帶有緊急拍停按鈕，平板電腦顯示，運動切換單元，更強的安全保護，可以實現(xiàn)浮游和爬行兩種功能的模塊化控制系統(tǒng)，以及方便可靠、低消耗的運動控制方法。
[0006]本發(fā)明的目的通過以下技術(shù)方案予以實現(xiàn):
[0007]一種水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)，包括水面系統(tǒng)、水下系統(tǒng)，所述水面系統(tǒng)放置在岸邊或者母船上，水下系統(tǒng)安裝在水下結(jié)構(gòu)物檢測機器人上；所述水面系統(tǒng)包括水面控制臺1、水面通信收發(fā)器2、臍帶纜3，所述水面控制臺I與水面通信收發(fā)器2相連，所述水面通信收發(fā)器2與臍帶纜3相連；
[0008]所述水下系統(tǒng)包括水下通信收發(fā)器4、供電單元5、嵌入式微控制器6、動力推進單元7、視覺照明單元8、運動切換單元9、安全保護單元10、傳感器單元11 ;所述水下通信收發(fā)器4與臍帶纜3相連，接收來自水面系統(tǒng)的控制信息，所述嵌入式微控制器6與水下通信收發(fā)器4相連，所述供電單元5、動力推進的單元7、控制視覺照明單元8、運動切換單元9、安全保護單元10、傳感器單元11分別與嵌入式微控制器6相連，所述供電單元5給水下系統(tǒng)供電，動力推進單元7接收嵌入式微控制器6的速度和方向信息，驅(qū)動螺旋槳旋轉(zhuǎn)，視覺照明單元8采集視頻以及接收來自嵌入式微控制器6的云臺和照明燈信號，運動切換單元9用以改變水下機器人的運動方式，安全保護單元10檢測機器人內(nèi)部環(huán)境信息發(fā)送給嵌入式微控制器6，并作數(shù)據(jù)存儲，傳感器單元11采集深度、導(dǎo)航、姿態(tài)、聲吶數(shù)據(jù)傳輸給嵌入式微控制器6進行控制；所述嵌入式微控制器6采集水下系統(tǒng)各單元數(shù)據(jù)，經(jīng)由水下通信收發(fā)器4發(fā)送到水面系統(tǒng)。
[0009]一種水下結(jié)構(gòu)物檢測機器人運動控制方法，該方法使用廣義預(yù)測PID控制算法對機器人的深度和航向角進行控制以提高機器人的穩(wěn)定性，具體步驟如下:
[0010]第一步:水下機器人采用受控自回歸積分滑動平均模型，即CARIMA模型描述:

【權(quán)利要求】
1.一種水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)，包括水面系統(tǒng)、水下系統(tǒng)，所述水面系統(tǒng)放置在岸邊或者母船上，水下系統(tǒng)安裝在水下結(jié)構(gòu)物檢測機器人上；其特征在于，所述水面系統(tǒng)包括水面控制臺(I)、水面通信收發(fā)器(2)、臍帶纜(3)，所述水面控制臺(I)與水面通信收發(fā)器(2)相連，所述水面通信收發(fā)器(2)與臍帶纜(3)相連； 所述水下系統(tǒng)包括水下通信收發(fā)器(4)、供電單元(5)、嵌入式微控制器￠)、動力推進單元(7)、視覺照明單元(8)、運動切換單元(9)、安全保護單元(10)、傳感器單元(11);所述水下通信收發(fā)器(4)與臍帶纜(3)相連，接收來自水面系統(tǒng)的控制信息，所述嵌入式微控制器(6)與水下通信收發(fā)器(4)相連，所述供電單元(5)、動力推進的單元(7)、控制視覺照明單元(8)、運動切換單元(9)、安全保護單元(10)、傳感器單元(11)分別與嵌入式微控制器(6)相連，所述供電單元(5)給水下系統(tǒng)供電，動力推進單元(7)接收嵌入式微控制器(6)的速度和方向信息，驅(qū)動螺旋槳旋轉(zhuǎn)，視覺照明單元(8)采集視頻以及接收來自嵌入式微控制器(6)的云臺和照明燈信號，運動切換單元(9)用以改變水下機器人的運動方式，安全保護單元(10)檢測機器人內(nèi)部環(huán)境信息發(fā)送給嵌入式微控制器(6)，并作數(shù)據(jù)存儲，傳感器單元(11)采集深度、導(dǎo)航、姿態(tài)、聲吶數(shù)據(jù)傳輸給嵌入式微控制器(6)進行控制；所述嵌入式微控制器(6)采集水下系統(tǒng)各單元數(shù)據(jù)，經(jīng)由水下通信收發(fā)器(4)發(fā)送到水面系統(tǒng)。
2.如權(quán)利要求1所述的水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)，其特征在于，所述水面控制臺(I)包括水面控制電腦(102)、緊急拍停開關(guān)(103)、操縱搖桿(104)、鍵盤鼠標(105)、液晶顯示器(106)、平板電腦(107);所述緊急拍停開關(guān)(103)與水面控制電腦(102)相連，在緊急情況下按下緊急拍停開關(guān)(103)，系統(tǒng)切斷電源停止控制；操縱搖桿(104)與水面控制電腦(102)相連，操縱機器人前進、后退、旋轉(zhuǎn)、上浮、下潛動作，鍵盤鼠標(105)與水面控制電腦(102)相連，輸入控制信息；液晶顯示器(106)實時顯示水面控制電腦(102)安裝的上位機軟件界面，平板電腦(107)通過無線通信與水面控制電腦(102)連接，移動訪問機器人控制信息，輔助非操作人員決策；水面控制電腦(102)與水面通信收發(fā)器(2)相連，進行數(shù)據(jù)收發(fā)。
3.如權(quán)利要求1所述的水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)，其特征在于，所述動力推進單元(7)包括光電隔離模塊(701)、電機驅(qū)動模塊(702)、吸附推進器電機(703)、首部垂向電機(704)、尾部垂向電機(705)、左側(cè)縱向電機(706)、右側(cè)縱向電機(707)、吸附螺旋槳(708)、首部垂向螺旋槳(709)、尾部垂向螺旋槳(710)、左側(cè)縱向螺旋槳(711)、左主動車輪(712)、右側(cè)縱向螺旋槳(713)、右主動車輪(714);所述光電隔離模塊(701)的輸出端接電機驅(qū)動模塊(702)的輸入端，電機驅(qū)動模塊(702)的輸出端接吸附推進器電機(703)、首部垂向電機(704)、尾部垂向電機(705)、左側(cè)縱向電機(706)、右側(cè)縱向電機(707)，所述吸附推進器電機(703)驅(qū)動吸附螺旋槳(708)，所述首部垂向電機(704)驅(qū)動首部垂向螺旋槳(709)，所述尾部垂向電機(705)驅(qū)動尾部垂向螺旋槳(710)，所述左側(cè)縱向電機(706)驅(qū)動左側(cè)縱向螺旋槳(711)、左主動車輪(712)，所述右側(cè)縱向電機(707)驅(qū)動右側(cè)縱向螺旋槳(713)、右主動車輪(714);所述嵌入式微控制器(6)發(fā)出的速度信號PWM和方向信號DIR，通過光電隔離模塊(701)進行信號隔離，將隔離后的信號傳給電機驅(qū)動模塊(702)，電機驅(qū)動模塊(702)輸出端控制吸附推進器電機(703)、首部垂向電機(704)、尾部垂向電機(705)、左側(cè)縱向電機(706)、右側(cè)縱向電機(707)的速度和方向來控制吸附螺旋槳(708)、首部垂向螺旋槳(709)、尾部垂向螺旋槳(710)、左側(cè)縱向螺旋槳(711)、左主動車輪(712)、右側(cè)縱向螺旋槳(713)、右主動車輪(714)的旋轉(zhuǎn)。
4.如權(quán)利要求1所述的水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)，其特征在于，所述運動切換單元(9)包括上部磁性接近開關(guān)(901)、下部磁性接近開關(guān)(902)、步進電機(903)、磁鐵(904)、滑臺(905)、絲杠(906)、導(dǎo)軌(907);所述左側(cè)縱向電機(706)和右側(cè)縱向電機(707)安裝在一個可以垂直活動的滑臺(905)上，步進電機(903)與滑臺(905)通過絲杠(906)連接，可以驅(qū)動滑臺(905)沿導(dǎo)軌(907)上下垂直移動，上部磁性接近開關(guān)(901)與下部磁性接近開關(guān)(902)分別安裝在固定滑臺架子的上部和下部，通過滑臺上部和下部的磁鐵(904)檢測滑臺(905)位置，判斷切換爬行和浮游狀態(tài)，實現(xiàn)運動切換功能。
5.如權(quán)利要求1所述的水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)，其特征在于，所述安全保護單元(10)包括電壓電流檢測模塊(1001)、漏水檢測模塊(1002)、溫濕度計(1003)、數(shù)據(jù)存儲卡(1004);所述電壓電流檢測模塊(1001)、漏水檢測模塊(1002)、溫濕度計(1003)、數(shù)據(jù)存儲卡(1004)與嵌入式微控制器(6)連接。
6.如權(quán)利要求1所述的水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)，其特征在于，所述傳感器單元(11)包括深度傳感器(1101)、導(dǎo)航定位模塊(1102)、姿態(tài)傳感器(1103)、聲吶傳感器(1104);所述深度傳感器(1101)采集機器人深度信息，當機器人浮在水面時用導(dǎo)航定位模塊(1102)確定和校正位置，所述姿態(tài)傳感器(1103)實時采集機器人姿態(tài)信息，包括橫滾角、俯仰角、航向角，所述聲吶傳感器(1104)采集水下結(jié)構(gòu)物信息，所述深度傳感器(1101)、導(dǎo)航定位模塊(1102)、姿態(tài)傳感器(1103)、聲吶傳感器(1104)與嵌入式微控制器(6)相連，將數(shù)據(jù)傳給嵌入式微控制器(6)。
7.如權(quán)利要求1所述的水下結(jié)構(gòu)物檢測機器人控制系統(tǒng)的運動控制方法，其特征在于，該方法使用廣義預(yù)測PID控制算法對機器人的深度和航向角進行控制以提高機器人的穩(wěn)定性，具體步驟如下: 第一步:水下機器人采用受控自回歸積分滑動平均模型，即CARIMA模型描述:
其中Αα-1)』。1)、。。1)是后移算子ζ—1的多項式，y(t) =時刻深度h和航向角yaw的矩陣，u(t-Ι)是t-1時刻推進器輸入電壓，Δ = l-z—1表示差分算子,e(t)為均值為零的白噪聲序列； 令C(z_i) = I單位矩陣，⑴式變?yōu)?
其中Ap^^，…，^，^^，盡，...，^是I旲型系數(shù)，na, nb分別為輸出、輸入的后移算子多項式階次，對于(2)式方程，引入D1phantine丟番圖方程:
I= Ej (ζ_1) Α(ζ_1) Δ +Z-jFj (ζ_1) 式中，Ej-(ζ—O = θ^+θ^ζ_1+...+θ^ej0 = I FJ Μ) = fJO+fJlf1 +.?η用⑴式求出k時刻后j步的輸出預(yù)測方程(3): y (k+j) = Gj(z_1) Au(k+j-l)+ym(3) 其中y(k+j)為k+j時刻深度和航向角預(yù)測輸出，△iKk+j-l)是推進器增量，其中令Gj (z-1) = Ej (z-1) Bj (z-1), ym = Fj (z_1) y (k)； 第二步:選取以下初始值:遺忘因子P，P取0.95~I ;輸出柔化系數(shù)α，α取0.75 ;輸入柔化系數(shù)β, β取0.4 ;正定矩陣P (-1);系數(shù)矩陣Θ (O)； 第三步:在線估計Α(ζ<)、Β(ζ_0: 令ε (k) = Ay(k)-X(k-1)T9 (k_l)，X(k-l)為過去時刻輸入，Ay(k)為當前輸出變化， 模型的系數(shù)
其中鞏幻二丨~⑷叫⑷.....a, (k).^(k).^(A)-...,/>, (!<)I'，由此得到變化后的A(z-1)、B(P)； 第四步:遞推Ε」(ζ_0、Fj(z_0:
Ej+1 (z_1) = Ej (z_1) +ejZ_J
Fj+l (z~l) = z(Fj (X1) — ejA) 式中，Z =Ej(^Fj(J)表示不斷更新的兩個變量； 第五步:計算Gj (z—1)、ym ； 根據(jù)上述推算可求得:
Gj(z_i) =Ej(z_i)Bj(z_i)
Ym = Fj (z_1) y (k) 第六步:引入輸入柔化系數(shù)β ;
令β≥0且β≤1，最優(yōu)取為0.4 ;
從而，式(3)變?yōu)?y (k+j) = Gj (z_1) H Δ u (k) +ym = L Δ u (k) +ym式中，H= [1，1+β，...，1+β+...n'L = Gj(Z^)H = [I1 I2...lj'n 為預(yù)測步長與時滯數(shù)之差； 第七步:對輸入u (k+j)、Au (k+j)添加約束條件:
Umin ^ u (k+j) ^ umax, Δ umin ^ Au (k+j)≤ Aumax
第八步:設(shè)定參考軌跡yr(k+l): 系統(tǒng)輸出不直接跟蹤設(shè)定值r (k)，而是跟蹤參考軌跡1τ (k+1)，采用如下一階濾波: yr (k) = y (k)
yr (k+1) = a yr(k) + (l-a )r(k) 第九步:求取目標函數(shù)Δ u (k),控制量ua(k+1)； 在上述約束條件下，取極小化性能指標 J = min { (yr(k)-y (k))T(yr (k+1)-y(k+1))} = min { (yr(k+1)-ym-L Δ u)T(yr (k+l)-ym-LAu)} 求得
Δ u(k) = (LtL) ^1Lt (yr (k+l)-ym) = Lt (yr (k+1)-ym) / Σ I2
控制量為 ua (k+1) = u (k) + Δ u (k) (6) 第十步:PID控制過程，以Ay(k)作為PID輸入，計算出下一刻控制量ub (k+1)； ub (k+1) = u (k) +Kp (Ay2-A Y1) +Ki Δ y2+Kd ( Δ y2-2 Δ Y1+ Δ y0) (7)
式中，Δγ2 = y (k+l)-y (k) ,Ay1 = y (k) -y (k~l) ,Ay0 = y (k-l)-y (k~2)，Kp、K1、Kd 分別為比例、積分、微分系數(shù)； 第十一步:對(6)式、(7)式計算的控制量均值濾波，求出水下機器人下一時刻推進器的輸入u (k+Ι)；
u (k+1) = (ua (k+1)+ub (k+1) )/2 所求u(k+l)即為水下機器人下一時刻推進器的輸入； 第十二步:t = t+1,返回到第三步,更新各項數(shù)據(jù)。
【文檔編號】G05D1/02GK104199447SQ201410407943
【公開日】2014年12月10日 申請日期:2014年8月18日 優(yōu)先權(quán)日:2014年8月18日
【發(fā)明者】曾慶軍, 眭翔, 張明 申請人:江蘇科技大學(xué)