本發(fā)明屬于自動控制技術(shù)領(lǐng)域，特別是涉及一種連續(xù)型機器人姿態(tài)控制系統(tǒng)。

背景技術(shù)：

傳統(tǒng)的離散型機器人均采用剛性關(guān)節(jié)和連桿結(jié)構(gòu)，用于實現(xiàn)在自由空間內(nèi)的多自由度運動，但由于其自由度數(shù)目有限，因此對工作空間受限的環(huán)境適應(yīng)性不強。與離散型機器人不同，連續(xù)型機器人為“無脊椎”的柔性結(jié)構(gòu)，該機器人采用形狀可以靈活改變的結(jié)構(gòu)，而不具有任何剛性的關(guān)節(jié)和連桿。這種新型的仿生機器人具有良好的彎曲性能，可以柔順而靈活地改變自身的形狀，因此具有能根據(jù)環(huán)境障礙物的狀況而改變自身形狀的能力，對工作空間受限的環(huán)境具有獨特的適應(yīng)能力。另外，其應(yīng)用前景廣闊，可以應(yīng)用于飛機油箱檢查、多障礙物工業(yè)環(huán)境內(nèi)的作業(yè)、彎曲管道和塌陷建筑物內(nèi)的偵查和搜救、核電站內(nèi)部管路的維護、人體疾病的診療等場合。

對連續(xù)型機器人的空間運動進行控制時，首先需要對機器人到達目標區(qū)域的行進路徑進行規(guī)劃，然后實現(xiàn)對給定的路徑進行跟隨。連續(xù)型機器人采用線驅(qū)動的方式，可以通過電機控制驅(qū)動線線長變化，從而使機器人到達預(yù)定位置。然而，控制過程中因傳動誤差、驅(qū)動線的張緊程度不同及控制模型不準確帶來的誤差等因素的影響，導(dǎo)致機器人位置控制誤差較大，為了實現(xiàn)連續(xù)型機器人對所規(guī)劃路徑的跟隨，前提是能控制機器人準確到達設(shè)定的目標位置。但目前尚缺少能夠?qū)B續(xù)型機器人的位置進行高精度控制的姿態(tài)控制系統(tǒng)。

技術(shù)實現(xiàn)要素：

為了解決上述問題，本發(fā)明的目的在于提供一種連續(xù)型機器人姿態(tài)控制系統(tǒng)。

為了達到上述目的，本發(fā)明提供的連續(xù)型機器人姿態(tài)控制系統(tǒng)包括核心控制器、蛇臂控制模塊、伸縮機構(gòu)控制模塊、蛇臂姿態(tài)采集模塊、驅(qū)動線拉力采集模塊、微處理器、姿態(tài)傳感器和壓力傳感器；其中核心控制器上帶有液晶顯示器、鼠標和鍵盤，與蛇臂姿態(tài)采集模塊、驅(qū)動線拉力采集模塊和微處理器相連接；蛇臂控制模塊連接在微處理器和連續(xù)型機器人上的蛇臂電機之間，用于控制驅(qū)動連續(xù)型機器人運動的蛇臂電機的速度和位置，從而改變驅(qū)動線長度，進而改變蛇臂姿態(tài)；伸縮機構(gòu)控制模塊連接在微處理器和連續(xù)型機器人上的直線模組電機之間，用于控制直線模組電機的速度和位置，進而驅(qū)動連續(xù)型機器人沿直線模組進行一維運動；蛇臂姿態(tài)采集模塊與安裝在蛇臂每段關(guān)節(jié)末端處的姿態(tài)傳感器相連接，用于將姿態(tài)傳感器檢測的姿態(tài)角數(shù)據(jù)通過串口傳輸至核心控制器，以形成姿態(tài)反饋的閉環(huán)控制；驅(qū)動線拉力采集模塊與安裝在蛇臂每個關(guān)節(jié)段末端圓盤上的壓力傳感器相連接，用于采集連續(xù)型機器人上每根驅(qū)動線的拉力值，并通過串口傳輸至核心控制器，以形成拉力反饋的閉環(huán)控制。

所述的蛇臂姿態(tài)采集模塊采用九軸慣性導(dǎo)航模塊JY-901，其內(nèi)部集成磁力計、陀螺儀和加速度計。

所述的壓力傳感器采用薄膜壓力傳感器。

本發(fā)明提供的連續(xù)型機器人姿態(tài)控制系統(tǒng)具有如下有益效果：

(1)結(jié)構(gòu)簡便，易于搭建

系統(tǒng)采用的姿態(tài)傳感器和薄膜壓力傳感器體積小，重量輕，模塊集成化程度高，對機器人蛇臂部分形變的影響小。

(2)位置控制精度高、可靠性好

系統(tǒng)既包含機器人蛇臂的姿態(tài)反饋，還包括驅(qū)動線力的反饋，具有位置控制精度高，可靠性好的特點。

附圖說明

圖1為連續(xù)型機器人單關(guān)節(jié)段運動學(xué)模型。

圖2為本發(fā)明提供的連續(xù)型機器人姿態(tài)控制系統(tǒng)構(gòu)成框圖。

圖3為蛇臂單關(guān)節(jié)段姿態(tài)和驅(qū)動力反饋控制圖。

具體實施方式

下面結(jié)合附圖和具體實施例對本發(fā)明提供的連續(xù)型機器人運動控制系統(tǒng)進行詳細說明。

1.1運動學(xué)分析

1.1.1單關(guān)節(jié)段運動學(xué)模型

對于線驅(qū)動連續(xù)型機器人，通過對蛇臂單關(guān)節(jié)模型的分析，如圖1所示，可以得到單關(guān)節(jié)基座坐標系至末端坐標系的齊次變換矩陣，如下式：

式中，s為正弦函數(shù)sin的縮寫，c為余弦函數(shù)cos的縮寫，θ∈(0,π]表示關(guān)節(jié)段的彎曲角度，表示關(guān)節(jié)段的旋轉(zhuǎn)角度，L為機器人單關(guān)節(jié)段長度。如此，通過齊次變化矩陣，建立了關(guān)節(jié)變量(彎曲角度和旋轉(zhuǎn)角度)與關(guān)節(jié)末端點位置之間的映射關(guān)系。

1.1.2多關(guān)節(jié)段運動學(xué)模型

將蛇臂機構(gòu)描述為多個連續(xù)型單關(guān)節(jié)的串聯(lián)，假設(shè)j#關(guān)節(jié)坐標系相對于(j-1)#關(guān)節(jié)坐標系的齊次變換矩陣為那么，關(guān)節(jié)末端坐標系{N}相對于系統(tǒng)坐標系的齊次變換矩陣可以表示為公式：其中，

θ_j為第j#關(guān)節(jié)的彎曲角度，為第j#關(guān)節(jié)的旋轉(zhuǎn)角度。如此，建立了多關(guān)節(jié)的關(guān)節(jié)變量與關(guān)節(jié)端點位置矢量之間的映射關(guān)系，即為多關(guān)節(jié)的運動學(xué)模型。

1.2本發(fā)明提供的連續(xù)型機器人姿態(tài)控制系統(tǒng)整體設(shè)計

如圖2所示，本發(fā)明提供的連續(xù)型機器人姿態(tài)控制系統(tǒng)包括核心控制器1、蛇臂控制模塊2、伸縮機構(gòu)控制模塊3、蛇臂姿態(tài)采集模塊4、驅(qū)動線拉力采集模塊5、微處理器6、姿態(tài)傳感器7和壓力傳感器8；其中核心控制器1上帶有液晶顯示器、鼠標和鍵盤，與蛇臂姿態(tài)采集模塊4、驅(qū)動線拉力采集模塊5和微處理器6相連接；蛇臂控制模塊2連接在微處理器6和連續(xù)型機器人上的蛇臂電機9之間，用于控制驅(qū)動連續(xù)型機器人運動的蛇臂電機9的速度和位置，從而改變驅(qū)動線長度，進而改變蛇臂姿態(tài)；伸縮機構(gòu)控制模塊3連接在微處理器6和連續(xù)型機器人上的直線模組電機10之間，用于控制直線模組電機10的速度和位置，進而驅(qū)動連續(xù)型機器人沿直線模組進行一維運動；蛇臂姿態(tài)采集模塊4與安裝在蛇臂每段關(guān)節(jié)末端處的姿態(tài)傳感器7相連接，用于將姿態(tài)傳感器7檢測的姿態(tài)角數(shù)據(jù)通過串口傳輸至核心控制器1，以形成姿態(tài)反饋的閉環(huán)控制；驅(qū)動線拉力采集模塊5與安裝在蛇臂每個關(guān)節(jié)段末端圓盤上的壓力傳感器8相連接，用于采集連續(xù)型機器人上每根驅(qū)動線的拉力值，并通過串口傳輸至核心控制器1，以形成拉力反饋的閉環(huán)控制。

所述的蛇臂姿態(tài)采集模塊4采用九軸慣性導(dǎo)航模塊JY-901，其內(nèi)部集成磁力計、陀螺儀和加速度計。

所述的壓力傳感器8采用薄膜壓力傳感器。這是因為采用卷繞方式控制驅(qū)動線線長變化時，驅(qū)動線是卷繞在卷軸上，因此很難在卷軸側(cè)安裝測量拉力的壓力傳感器，而驅(qū)動線的另一端是固定在蛇臂每個關(guān)節(jié)段的末端圓盤上，因此可在該處加裝壓力傳感器8。

現(xiàn)將本發(fā)明提供的連續(xù)型機器人姿態(tài)控制系統(tǒng)工作原理闡述如下：首先由核心控制器1接受位置指令，經(jīng)過算法解算后將三維位置量轉(zhuǎn)換為蛇臂的關(guān)節(jié)變量(即旋轉(zhuǎn)角度和彎曲角度)，然后通過串口發(fā)送給微處理器6；微處理器6將關(guān)節(jié)變量解算為繩長變化量和上升距離進而轉(zhuǎn)化為電機的轉(zhuǎn)動行程，分別發(fā)送給蛇臂控制模塊2和伸縮機構(gòu)控制模塊3；由蛇臂控制模塊2和伸縮機構(gòu)控制模塊3分別驅(qū)動蛇臂電機9和直線模組電機10的運動，最終驅(qū)動機器人蛇臂到達目標點。在運動過程中本姿態(tài)控制系統(tǒng)實時采集蛇臂的狀態(tài)，其中蛇臂姿態(tài)采集模塊4通過姿態(tài)傳感器7采集機器人蛇臂每段關(guān)節(jié)段末端處的姿態(tài)角數(shù)據(jù)，然后傳送給核心控制器1，核心控制器1經(jīng)過算法解算后便可以得到各個關(guān)節(jié)段的彎曲角度和旋轉(zhuǎn)角度，然后得到與輸入的關(guān)節(jié)變量之間的差值。將此關(guān)節(jié)變量差值解算為繩長變化量，然后實時發(fā)出調(diào)整指令來實現(xiàn)蛇臂姿態(tài)快速準確調(diào)節(jié)。與此同時，驅(qū)動線拉力采集模塊4通過壓力傳感器8實時采集每一個蛇臂關(guān)節(jié)段末端三根驅(qū)動線的拉力值，然后傳送給核心控制器1，核心控制器1設(shè)定拉力閾值，并根據(jù)拉力的從小到大做出不同輸出。其中在很小的拉力下輸出零，代表不干預(yù)當前進程；在可控拉力范圍內(nèi)輸出調(diào)整信號；當拉力超過最大設(shè)定值時，發(fā)出系統(tǒng)中斷信號，停止運行并進行報警提示。這樣通過蛇臂姿態(tài)反饋和驅(qū)動線拉力反饋，機器人就能在快速準確到達設(shè)定位置的前提下同時保證驅(qū)動線處于安全拉力范圍以內(nèi)。

1.3基于姿態(tài)和驅(qū)動力反饋的機器人控制算法設(shè)計

1.3.1末端姿態(tài)角求解

在對連續(xù)型機器人單關(guān)節(jié)段進行建模時，末端坐標系相對于首端坐標系的旋轉(zhuǎn)變換為按照Z-Y-Z次序，而姿態(tài)傳感器測量的姿態(tài)角為RPY角，固結(jié)在姿態(tài)傳感器7上的坐標系相對于初始位置坐標系的變換按照Z-Y-X旋轉(zhuǎn)次序，不同歐拉變換表示坐標系旋轉(zhuǎn)變換時的歐拉角是不同的，因此，無法直接使用姿態(tài)傳感器7測量的RPY角進行控制。對機器人關(guān)節(jié)段進行閉環(huán)控制需要已知當前的關(guān)節(jié)變量，故需將測量得到的ZYX歐拉角轉(zhuǎn)換為ZYZ歐拉角。

由于按照不同的旋轉(zhuǎn)次序均可實現(xiàn)坐標系的空間變換，故旋轉(zhuǎn)變換矩陣相同，可利用姿態(tài)傳感器測量的ZYX歐拉角求出變換矩陣，進而求解ZYZ歐拉角。

設(shè)坐標系變換的ZYX歐拉角為(ω,ξ,ψ)，則與RPY角對應(yīng)關(guān)系為：

可求得旋轉(zhuǎn)變換矩陣為：

設(shè)坐標系變換的ZYZ歐拉角為(α,β,γ)，則旋轉(zhuǎn)變換矩陣為：

令R(α,β,γ)＝R(ω,ξ,ψ)，可求出ZYZ歐拉角(α,β,γ)。為簡便表示，將R(ω,ξ,ψ)寫作：

當sβ≠0時，可求出ZYZ歐拉角(α,β,γ)為：

式中，A tan2(y,x)為雙變量反正切函數(shù)，是計算arctan(y/x)，并能根據(jù)x和y的符號判斷角度所在的象限。

當β＝0°時，可求出ZYZ歐拉角(α,β,γ)為：

當β＝180°時，可求出ZYZ歐拉角(α,β,γ)為：

末端坐標系相對于首端坐標系繞ZYZ軸的旋轉(zhuǎn)角度依次為故應(yīng)滿足下式：

理論上計算出的角度應(yīng)滿足α＝-γ

在連續(xù)型機器人姿態(tài)控制中使用(β,-γ)作為反饋的關(guān)節(jié)變量

1.3.2蛇臂姿態(tài)和驅(qū)動力反饋設(shè)計

基于姿態(tài)和驅(qū)動力反饋的蛇臂運動閉環(huán)控制器控制框圖如圖3所示。以機器人蛇臂關(guān)節(jié)變量作為系統(tǒng)的輸入，姿態(tài)傳感器7實時測量機器人蛇臂末端的姿態(tài)角數(shù)據(jù)，并通過蛇臂姿態(tài)采集模塊4傳送給核心控制器1，核心控制器1經(jīng)過歐拉角變換轉(zhuǎn)換為ZYZ歐拉角，可得當前蛇臂的關(guān)節(jié)變量(β,-γ)，求解關(guān)節(jié)變量的差值并計算驅(qū)動線線長變化量。在核心控制器1中設(shè)計機器人姿態(tài)PID控制器，以使機器人蛇臂姿態(tài)調(diào)節(jié)快速、準確；同時設(shè)計驅(qū)動線拉力限幅濾波器和驅(qū)動線拉力PID控制器，設(shè)定拉力最小值F_min、拉力過大調(diào)節(jié)值F_high和最大值F_max，驅(qū)動線拉力F處于不同范圍時，驅(qū)動線拉力限幅濾波器給出相應(yīng)輸出ΔF。當壓力傳感器7實時檢測的驅(qū)動線拉力F＜F_min或F_high≤F≤F_max時，驅(qū)動線拉力限幅濾波器有輸出，通過驅(qū)動線拉力PID控制器進行調(diào)節(jié)；當F_min≤F≤F_high時，拉力處于合適的范圍內(nèi)，驅(qū)動線拉力限幅濾波器輸出為0，不進行調(diào)整；當拉力F＞F_max時，驅(qū)動線拉力限幅濾波器不輸出，系統(tǒng)中斷運行，進行報警提示。通過蛇臂姿態(tài)和驅(qū)動線拉力反饋，實現(xiàn)蛇臂快速準確到達設(shè)定的姿態(tài)，并且運行中驅(qū)動線始終處于合適的拉力范圍內(nèi)。