本發(fā)明涉及機(jī)器人技術(shù)領(lǐng)域,尤其是涉及一種水下移動(dòng)作業(yè)機(jī)器人的協(xié)調(diào)規(guī)劃與控制方法。
背景技術(shù):
廣闊的海洋為人類提供了豐富的資源,對(duì)海洋資源的高效利用和可持續(xù)開發(fā)對(duì)人類具有重要的意義。人類對(duì)海洋的探索從未停止,從最原始的載人水下航行器,到遙控型水下航行器,再到如今的全自主無(wú)人水下航行器的研究,這些極大地推動(dòng)了人類對(duì)海洋的認(rèn)識(shí)和開發(fā)。特別是在海產(chǎn)養(yǎng)殖中,我國(guó)近淺海水產(chǎn)養(yǎng)殖業(yè)迅猛發(fā)展,各類水產(chǎn)品產(chǎn)量巨大,由于捕撈技術(shù)落后,卻面臨著效率低、淺海環(huán)境污染、植被破壞及資源消耗嚴(yán)重的窘境。利用潛水員在水下作業(yè)采捕海珍品的人工采捕方式對(duì)環(huán)境無(wú)破壞,但這種方式的效率低,作業(yè)時(shí)間有限,作業(yè)環(huán)境惡劣,而且潛在危險(xiǎn)多等。隨著對(duì)海洋環(huán)境保護(hù)和工作效率要求的不斷提高,采用水下移動(dòng)作業(yè)機(jī)器人等采捕設(shè)備替代人工水下作業(yè)的需求愈發(fā)迫切。水下移動(dòng)作業(yè)機(jī)器人一般由移動(dòng)平臺(tái)和一個(gè)或多個(gè)機(jī)械臂組成,移動(dòng)平臺(tái)和機(jī)械臂按需求設(shè)計(jì)、制造后組裝集成在一起,通過(guò)高度集成的感知、規(guī)劃與控制系統(tǒng),實(shí)現(xiàn)機(jī)器人的水下自主作業(yè)。在非結(jié)構(gòu)化未知的水下環(huán)境,水下移動(dòng)作業(yè)機(jī)器人實(shí)現(xiàn)全自主作業(yè)主要分為自主巡游與自主作業(yè)兩個(gè)階段。歐美及其日本研制各類型的水下機(jī)器人作業(yè)系統(tǒng),這些作業(yè)機(jī)器人普遍通過(guò)遙控或者半自主的方式進(jìn)行水下作業(yè),普遍存在自主化水平不足、操作技術(shù)要求高等弊端。
有鑒于此,特提出本發(fā)明。
技術(shù)實(shí)現(xiàn)要素:
為了解決現(xiàn)有技術(shù)中的上述問(wèn)題,即為了解決如何實(shí)現(xiàn)水下移動(dòng)作業(yè)機(jī)器人全自主巡游與作業(yè)的技術(shù)問(wèn)題,提供一種水下移動(dòng)作業(yè)機(jī)器人的協(xié)調(diào)規(guī)劃與控制方法。
為了實(shí)現(xiàn)上述目的,提供以下技術(shù)方案:
一種水下移動(dòng)作業(yè)機(jī)器人的協(xié)調(diào)規(guī)劃與控制方法,該方法包括:
通過(guò)動(dòng)態(tài)追蹤微分器,實(shí)時(shí)規(guī)劃當(dāng)前期望的速度與狀態(tài);
采用迭代任務(wù)優(yōu)先方法,將笛卡爾空間的任務(wù)規(guī)劃轉(zhuǎn)化到隨體坐標(biāo)系和各關(guān)節(jié)坐標(biāo)系的速度與加速度規(guī)劃;
根據(jù)速度與加速度規(guī)劃,利用動(dòng)力學(xué)方法來(lái)控制潛器和作業(yè)臂,從而使得水下移動(dòng)作業(yè)機(jī)器人進(jìn)行巡游與作業(yè)。
優(yōu)選地,通過(guò)動(dòng)態(tài)追蹤微分器,實(shí)時(shí)規(guī)劃當(dāng)前期望的速度與狀態(tài)具體包括:
利用自抗擾技術(shù),通過(guò)動(dòng)態(tài)追蹤微分器將階躍型任務(wù)信號(hào)轉(zhuǎn)化為連續(xù)的光滑信號(hào),并得到當(dāng)前期望的速度和狀態(tài)。
優(yōu)選地,根據(jù)下式來(lái)建立動(dòng)態(tài)追蹤微分器:
f=f(v1-v0,v2,r0,h0)
v1=v1+h0v2
v2=v2+h0f
其中,v0表示給定的期望目標(biāo)點(diǎn);v1表示狀態(tài)規(guī)劃;v2表示速度規(guī)劃;r0、h0分別表示影響微分器曲線的速度和光滑程度的微分器參數(shù);
其中,f根據(jù)下式確定:
優(yōu)選地,采用迭代任務(wù)優(yōu)先方法,將笛卡爾空間的任務(wù)規(guī)劃轉(zhuǎn)化到隨體坐標(biāo)系和各關(guān)節(jié)坐標(biāo)系的速度與加速度規(guī)劃,具體包括:
通過(guò)迭代任務(wù)優(yōu)先方法進(jìn)行冗余迭代,對(duì)笛卡爾空間世界坐標(biāo)系下任務(wù)規(guī)劃進(jìn)行轉(zhuǎn)化,求解在隨體坐標(biāo)和各關(guān)節(jié)坐標(biāo)系下的期望速度與期望加速度。
優(yōu)選地,方法還包括:
通過(guò)狀態(tài)觀測(cè)器觀測(cè)估計(jì)當(dāng)前的任務(wù)速度與任務(wù)狀態(tài),并進(jìn)行實(shí)時(shí)反饋,以更新速度與加速度規(guī)劃。
優(yōu)選地,實(shí)時(shí)反饋中的反饋信號(hào)通過(guò)以下方式來(lái)確定:
根據(jù)當(dāng)前規(guī)劃的期望速度和期望加速度、當(dāng)前速度誤差和當(dāng)前狀態(tài)誤差,并且與規(guī)劃的速度加權(quán)結(jié)合,來(lái)確定反饋信號(hào)。
優(yōu)選地,狀態(tài)觀測(cè)器通過(guò)下式來(lái)建立:
e=z1-z0
z1=z1+h0(z2-β01e)
z2=z2+h0(z3-β02e+Jζ)
z3=z3+h0(-β03e)
其中,β01、β02、β03表示設(shè)定的保證收斂的參數(shù);z0表示期望的目標(biāo)狀態(tài);z1、z2、z3分別表示對(duì)狀態(tài)、速度和加速度的觀測(cè)估計(jì);J表示相對(duì)應(yīng)的雅克比矩陣;ζ由任務(wù)速度規(guī)劃與誤差(狀態(tài)誤差與速度誤差)共同組成的反饋?lái)?xiàng);h0表示影響微分器曲線的光滑程度的微分器參數(shù)。
本發(fā)明實(shí)施例提供一種水下移動(dòng)作業(yè)機(jī)器人的協(xié)調(diào)規(guī)劃與控制方法。該方法包括:通過(guò)動(dòng)態(tài)追蹤微分器,實(shí)時(shí)規(guī)劃當(dāng)前期望的速度與狀態(tài);用迭代任務(wù)優(yōu)先方法,將笛卡爾空間的任務(wù)規(guī)劃轉(zhuǎn)化到隨體坐標(biāo)系和各關(guān)節(jié)坐標(biāo)系的速度與加速度規(guī)劃;根據(jù)速度與加速度規(guī)劃,利用動(dòng)力學(xué)方法來(lái)控制潛器和作業(yè)臂,從而使得水下移動(dòng)作業(yè)機(jī)器人進(jìn)行巡游與作業(yè)。本發(fā)明實(shí)施例將自主巡游和自主作業(yè)過(guò)程的各個(gè)子任務(wù)統(tǒng)一起來(lái),采用動(dòng)態(tài)追蹤微分器,實(shí)施規(guī)劃當(dāng)前各任務(wù)的期望速度與期望狀態(tài),然后,采用狀態(tài)觀測(cè)器,估計(jì)當(dāng)前任務(wù)狀態(tài)與速度,并反饋至控制輸入,最后通過(guò)魯棒的迭代任務(wù)優(yōu)先方法實(shí)現(xiàn)閉環(huán)控制,提高了水下移動(dòng)作業(yè)機(jī)器人的自主化水平,最終實(shí)現(xiàn)了水下移動(dòng)作業(yè)機(jī)器人全自主巡游與作業(yè)。
附圖說(shuō)明
圖1為根據(jù)本發(fā)明實(shí)施例的水下移動(dòng)機(jī)器人作業(yè)臂系統(tǒng)的示意圖;
圖2為根據(jù)本發(fā)明實(shí)施例的水下移動(dòng)作業(yè)機(jī)器人坐標(biāo)系示意圖;
圖3為根據(jù)本發(fā)明實(shí)施例的水下移動(dòng)作業(yè)機(jī)器人的協(xié)調(diào)規(guī)劃與控制方法的流程示意圖;
圖4為根據(jù)本發(fā)明實(shí)施例的帶有跟蹤微分器和狀態(tài)觀測(cè)器的魯棒迭代任務(wù)優(yōu)先算法的流程示意圖。
具體實(shí)施方式
下面參照附圖來(lái)描述本發(fā)明的優(yōu)選實(shí)施方式。本領(lǐng)域技術(shù)人員應(yīng)當(dāng)理解的是,這些實(shí)施方式僅僅用于解釋本發(fā)明的技術(shù)原理,并非旨在限制本發(fā)明的保護(hù)范圍。
本發(fā)明實(shí)施例基本構(gòu)思是將航行器艦體和機(jī)械臂做統(tǒng)一的規(guī)劃和控制,充分考慮艇體與機(jī)械臂的耦合作用,將自主作業(yè)的任務(wù)劃分為若干個(gè)子任務(wù),并將子任務(wù)做優(yōu)先級(jí)排列,然后,按照優(yōu)先級(jí)順序依次融合各子任務(wù)控制策略與方案,從而完成各子任務(wù)并實(shí)現(xiàn)最終的作業(yè)任務(wù)。
圖1是水下移動(dòng)機(jī)器人作業(yè)臂系統(tǒng),該系統(tǒng)主要由兩側(cè)對(duì)稱裝配的仿生推動(dòng)器、作業(yè)臂和艇體三部分組成。對(duì)稱的仿生推進(jìn)器可以提供機(jī)器人前進(jìn)、后退、側(cè)移、上浮、下潛、正逆旋轉(zhuǎn)所需要的力和力矩,即在本體坐標(biāo)系中具有四個(gè)自由度,如圖2所示,分別沿Ob-x,y,z方向的平移運(yùn)動(dòng)以及繞Ob-z的旋轉(zhuǎn)運(yùn)動(dòng)。在機(jī)械臂上,也存在四個(gè)自由度,分別是腰關(guān)節(jié)、肩關(guān)節(jié)、肘關(guān)節(jié)、腕關(guān)節(jié),分別可以做左右擺動(dòng)上下擺動(dòng)、上下擺動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng),機(jī)械臂末端為雙指夾器,通過(guò)鋼絲牽動(dòng)實(shí)現(xiàn)目標(biāo)的夾取等,各關(guān)節(jié)分別建立Oi-x,y,z,i=0,1,2,3坐標(biāo)系。
借助于機(jī)械臂運(yùn)動(dòng)學(xué)模型,給定任務(wù)與系統(tǒng)速度的關(guān)系可以表示為:
其中,是給定任務(wù)的時(shí)間微分;ζ為系統(tǒng)速度;J是相應(yīng)的雅克比矩陣。
在橫滾和俯仰角的控制上,僅僅通過(guò)長(zhǎng)鰭波動(dòng)實(shí)現(xiàn),相當(dāng)困難。除此之外,當(dāng)自主作業(yè)時(shí),如果帶有俯仰和橫滾角,自主作業(yè)的控制和實(shí)現(xiàn)也比較困難,因此在實(shí)際規(guī)劃和作業(yè)過(guò)程中,需要限制水下移動(dòng)作業(yè)機(jī)器人的俯仰角和橫滾角始終為零。
為此本發(fā)明實(shí)施例提供一種水下移動(dòng)作業(yè)機(jī)器人的協(xié)調(diào)規(guī)劃與控制方法。如圖3所示該方法可以包括:
S1:通過(guò)動(dòng)態(tài)追蹤微分器,實(shí)時(shí)規(guī)劃當(dāng)前期望的速度與狀態(tài)。
具體地,本步驟可以包括:利用自抗擾技術(shù),通過(guò)動(dòng)態(tài)追蹤微分器將階躍型任務(wù)信號(hào)轉(zhuǎn)化為連續(xù)的光滑信號(hào)并得到當(dāng)前期望的速度和狀態(tài)。
在實(shí)際應(yīng)用中,控制目標(biāo)往往是一種階躍信號(hào)形式,例如,到達(dá)指定的位置等,因此,輸出信號(hào)存在跳躍,嚴(yán)格的追蹤階躍軌跡非常困難,另外,系統(tǒng)反饋信號(hào)并不精確,因此基于自適應(yīng)抗擾動(dòng)技術(shù),來(lái)根據(jù)當(dāng)前的狀態(tài)與期望目標(biāo),規(guī)劃當(dāng)前的速度、狀態(tài)等信息。
在一個(gè)優(yōu)選的實(shí)施例中,可以根據(jù)下式來(lái)建立動(dòng)態(tài)追蹤微分器:
f=f(v1-v0,v2,r0,h0)
v1=v1+h0v2
v2=v2+h0f
其中,v0表示給定的期望目標(biāo)點(diǎn);v1表示狀態(tài)規(guī)劃;v2表示速度規(guī)劃或任務(wù)速度規(guī)劃;r0、h0分別表示影響微分器曲線的速度和光滑程度的微分器參數(shù);其中,f(·)根據(jù)下式確定:
a0=h0v2
y=v1+a0
a2=a0+sign(y)(a1-d)/2
sy=(sign(y+d)-sign(y-d))/2
a=(a0+y-a2)sy+a2
sa=(sign(a+d)-sign(a-d))/2
f=-r0(a/d-sign(a))sa-r0sign(a)
本發(fā)明實(shí)施例通過(guò)采用動(dòng)態(tài)追蹤微分器,將階躍型任務(wù)信號(hào)轉(zhuǎn)化為連續(xù)的光滑信號(hào),使得控制信號(hào)連續(xù)光滑,實(shí)現(xiàn)了信號(hào)的連續(xù)性和可微性,使得控制信號(hào)更加細(xì)致化,避免了信號(hào)的階躍型跳躍和震動(dòng);而且動(dòng)態(tài)追蹤微分器將信號(hào)平滑化的同時(shí),給出了當(dāng)前的速度規(guī)劃,并且實(shí)時(shí)計(jì)算出當(dāng)前的速度和狀態(tài)信息。
圖4示例性地示出了得到任務(wù)的狀態(tài)與速度規(guī)劃的示意圖。沿用前例,以3個(gè)任務(wù)(i=1,2,3)進(jìn)行示例性說(shuō)明。其中,對(duì)任務(wù)集σz并結(jié)合機(jī)器人的位姿η和機(jī)械臂各關(guān)節(jié)角q進(jìn)行運(yùn)動(dòng)學(xué)控制,得到系統(tǒng)參考速度vd與系統(tǒng)參考加速度然后根據(jù)vd、進(jìn)行動(dòng)力學(xué)控制,得到期望的力和力矩τd;然后,期望的力和力矩τd輸入至驅(qū)動(dòng)控制部分;驅(qū)動(dòng)控制部分輸出機(jī)器人艇體的速度v與加速度速度v與加速度再反饋至動(dòng)力學(xué)控制部分;σ1、σ2、σ3是給定任務(wù)中三個(gè)子任務(wù),通過(guò)追蹤微分器(TD1、TD2、TD3)之后,可以得到各個(gè)子任務(wù)的狀態(tài)與速度規(guī)劃。其中,i=1,2,3;σi為給定的第i個(gè)任務(wù),TDi為追蹤微分器(Tracking Differentiator),σid,為第i個(gè)任務(wù)的狀態(tài)規(guī)劃和速度規(guī)劃;Kip,Kid為狀態(tài)誤差和速度誤差的加權(quán)系數(shù);ζi是滿足第i個(gè)任務(wù)所需要的系統(tǒng)速度;Qi-1是零空間映射算子;ζ為滿足所有任務(wù)的系統(tǒng)速度;dζ/dt對(duì)速度進(jìn)行時(shí)間微分,得到加速度信息;v,為機(jī)器人艇體的速度與加速度;為機(jī)械臂各關(guān)節(jié)的旋轉(zhuǎn)速度和加速度,q為機(jī)械臂各關(guān)節(jié)角;η是機(jī)器人的位姿;SOi為狀態(tài)觀測(cè)器(State Observer);為第i個(gè)任務(wù)的狀態(tài)和速度的觀測(cè)。
S2:采用迭代任務(wù)優(yōu)先方法,將笛卡爾空間的任務(wù)規(guī)劃轉(zhuǎn)化到隨體坐標(biāo)系和各關(guān)節(jié)坐標(biāo)系的速度與加速度規(guī)劃。
其中,迭代任務(wù)優(yōu)先方法具有很好的魯棒性。目標(biāo)任務(wù)可以是在笛卡爾空間世界坐標(biāo)系中給定的。
作為另一個(gè)實(shí)施例,具體地,本步驟可以包括:通過(guò)迭代任務(wù)優(yōu)先方法進(jìn)行冗余迭代,對(duì)笛卡爾空間世界坐標(biāo)系下任務(wù)規(guī)劃進(jìn)行轉(zhuǎn)化,求解在隨體坐標(biāo)和各關(guān)節(jié)坐標(biāo)系下的期望速度與期望加速度。
本步驟采用任務(wù)優(yōu)先方法框架,將笛卡爾空間世界坐標(biāo)系下任務(wù)規(guī)劃轉(zhuǎn)化到隨體坐標(biāo)系和各關(guān)節(jié)坐標(biāo)系的速度與加速度規(guī)劃,并作為動(dòng)力學(xué)控制的信息輸入。
沿用圖4所示示例,σ1處于最高優(yōu)先級(jí),σ2次之,σ3優(yōu)先級(jí)最低,根據(jù)下式通過(guò)任務(wù)優(yōu)先方法進(jìn)行依次迭代:
ζ=v2+k1(v1-z1)+k2(v2-z2)
其中,ζ表示由任務(wù)速度規(guī)劃v2與誤差(狀態(tài)誤差與速度誤差)共同組成的反饋?lái)?xiàng);Q表示零空間映射算子;k1表示狀態(tài)誤差加權(quán)系數(shù);k2表示速度誤差加權(quán)系數(shù);z1表示對(duì)狀態(tài)的觀測(cè)估計(jì);z2表示對(duì)速度的觀測(cè)估計(jì);I表示單位矩陣;i表示當(dāng)前任務(wù)索引,即表示將所有給定任務(wù)依次進(jìn)行迭代,其最大值是給定的任務(wù)總數(shù);表示第i個(gè)任務(wù)的相應(yīng)雅克比矩陣的廣義逆矩陣;Ji表示第i個(gè)任務(wù)的相應(yīng)雅克比矩陣;ρi表示迭代輸出結(jié)果。
圖4示例性地示出了帶有跟蹤微分器和狀態(tài)觀測(cè)器的魯棒迭代任務(wù)優(yōu)先算法的流程示意圖。通過(guò)圖4所示方法,消除了經(jīng)典任務(wù)優(yōu)先算法的運(yùn)動(dòng)學(xué)奇異和算法奇異現(xiàn)象,保證了控制算法的魯棒性,本發(fā)明實(shí)施例可以將所有的任務(wù)依次迭代,計(jì)算出滿足所有任務(wù)需求的在隨體坐標(biāo)系和各關(guān)節(jié)坐標(biāo)系中的速度與加速度。
S3:根據(jù)速度與加速度規(guī)劃,利用動(dòng)力學(xué)方法來(lái)控制潛器和作業(yè)臂,從而產(chǎn)生水下移動(dòng)作業(yè)機(jī)器人的巡游與作業(yè)。
在一個(gè)優(yōu)選的實(shí)施例中,本發(fā)明實(shí)施例提供的方法還可以包括:通過(guò)狀態(tài)觀測(cè)器觀測(cè)估計(jì)當(dāng)前的任務(wù)速度與任務(wù)狀態(tài),并進(jìn)行實(shí)時(shí)反饋,以更新速度與加速度規(guī)劃。
其中,通過(guò)狀態(tài)觀測(cè)器觀測(cè)估計(jì)當(dāng)前的速度與狀態(tài),并實(shí)時(shí)反饋至輸入端,構(gòu)成閉環(huán)控制,從而可以提高本發(fā)明實(shí)施例的穩(wěn)定性。
在一個(gè)優(yōu)選的實(shí)施例中,實(shí)時(shí)反饋中的反饋信號(hào)通過(guò)以下方式來(lái)確定:根據(jù)當(dāng)前規(guī)劃的期望速度和期望加速度、當(dāng)前速度誤差和當(dāng)前狀態(tài)誤差,并且與規(guī)劃的速度加權(quán)結(jié)合,來(lái)確定反饋信號(hào)。
在一個(gè)優(yōu)選的實(shí)施例中,狀態(tài)觀測(cè)器通過(guò)下式來(lái)建立:
e=z1-z0
z1=z1+h0(z2-β01e)
z2=z2+h0(z3-β02e+Jζ)
z3=z3+h0(-β03e)
其中,β01、β02、β03表示設(shè)定的保證收斂的參數(shù);z1、z2、z3分別表示對(duì)狀態(tài)、速度和加速度的觀測(cè)估計(jì);z0表示期望的目標(biāo)狀態(tài);J表示相對(duì)應(yīng)的雅克比矩陣;ζ由任務(wù)速度規(guī)劃與誤差(狀態(tài)誤差與速度誤差)共同組成的反饋?lái)?xiàng);h0表示影響微分器曲線的光滑程度的微分器參數(shù)。
本發(fā)明實(shí)施例通過(guò)采用上述技術(shù)方案,實(shí)現(xiàn)了潛器-作業(yè)臂協(xié)調(diào)規(guī)劃與運(yùn)動(dòng)控制的閉環(huán)控制,解決了如何實(shí)現(xiàn)水下移動(dòng)作業(yè)機(jī)器人的自主巡游與作業(yè)的技術(shù)問(wèn)題。通過(guò)狀態(tài)觀測(cè)器可以實(shí)時(shí)地觀測(cè)估計(jì)系統(tǒng)的狀態(tài)和速度信息,從而大大提高了在末端自主作業(yè)時(shí)的穩(wěn)定性和精準(zhǔn)度;而且,反饋信號(hào)綜合考慮了當(dāng)前規(guī)劃的期望速度、當(dāng)前速度誤差及當(dāng)前狀態(tài)誤差,相比于單一的狀態(tài)誤差方法而言,其收斂速度更快;將自主巡游和自主作業(yè)兩個(gè)階段的不同任務(wù)做統(tǒng)一考慮,無(wú)需進(jìn)行階段劃分和狀態(tài)切換機(jī)制的設(shè)計(jì),可以同時(shí)考慮多個(gè)任務(wù)的同時(shí)執(zhí)行而且消除任務(wù)之間的相互影響,從而保證迭代更多的任務(wù),將所有的任務(wù)都統(tǒng)一在本框架之下,通過(guò)自適應(yīng)調(diào)節(jié)機(jī)制,能夠保證在不同階段對(duì)不同的性能指標(biāo)的要求,可以統(tǒng)一實(shí)現(xiàn)自主的高速高機(jī)動(dòng)自主巡游和高穩(wěn)定高精準(zhǔn)自主作業(yè),例如:在巡游階段的高速高機(jī)動(dòng)性,作業(yè)階段的高穩(wěn)定高精度性,消除了經(jīng)典任務(wù)優(yōu)先方法的運(yùn)動(dòng)學(xué)奇異和方法奇異,保證了方法的魯棒性。
至此,已經(jīng)結(jié)合附圖所示的優(yōu)選實(shí)施方式描述了本發(fā)明的技術(shù)方案,但是,本領(lǐng)域技術(shù)人員容易理解的是,本發(fā)明的保護(hù)范圍顯然不局限于這些具體實(shí)施方式。在不偏離本發(fā)明的原理的前提下,本領(lǐng)域技術(shù)人員可以對(duì)相關(guān)技術(shù)特征作出等同的更改或替換,這些更改或替換之后的技術(shù)方案都將落入本發(fā)明的保護(hù)范圍之內(nèi)。