滿文鏢,李艷杰,卜春光,高英麗,郎智明,眭晉
(1.沈陽理工大學(xué)機械工程學(xué)院,沈陽 110016; 2.中國科學(xué)院沈陽自動化研究所,沈陽 110016)
面向危樓傷員救援需要,基于移動操作協(xié)調(diào)技術(shù)路線,我團隊研制出傷員搬運機器人和運載機器人。搬運機器人負責將傷員從地面抬起并在合適的位置、高度移交給運載機器人,搬運機器人本體由2自由度履帶式移動機構(gòu)、5自由度機械臂和傷員搬運工具構(gòu)成,機器人運動自由度數(shù)總計為7個,運動自由度存在冗余。機器人搬運對象為傷員,為了提高搬運過程傷員的安全性、舒適性,如何規(guī)劃機器人搬運運動,確定自由度冗余情況下搬運機器人運動學(xué)逆解,控制機器人保持傷員、機器人重心穩(wěn)定,避免傾翻危險,是搬運機器人需要解決的關(guān)鍵核心問題。
關(guān)于冗余自由度機器人不同需求運動規(guī)劃的應(yīng)用,通過動力學(xué)模型的建立,文獻[1]、[2]分別提出時間最優(yōu)和能耗最優(yōu)的軌跡規(guī)劃方法;為實現(xiàn)實時軌跡規(guī)劃,文獻[3]基于雅可比矩陣提出了人工勢場軌跡規(guī)劃方法,文獻[4]利用虛擬排斥速度場和機械臂自運動特性實現(xiàn)了避障的運動規(guī)劃;文獻[5]考慮避障環(huán)境為封閉類管道下進行零件噴涂,提出基于碰撞反饋的冗余機械臂避障算法;文獻[6]通過改進麻雀搜索法對超冗余機械臂進行軌跡規(guī)劃;針對雙足多自由度機器人,文獻[7]實現(xiàn)18自由度雙足機器人的全身運動規(guī)劃,文獻[8]、[9]考慮傾斜地面,針對多自由度雙足機器人行走的最優(yōu)軌跡進行運動規(guī)劃。
針對傾翻問題,文獻[10]針對兩足機器人的運動穩(wěn)定性進行分析,文獻[11]針對兩足機器人接觸點的平衡穩(wěn)定性進行研究,文獻[12]針對兩足類人生物在不平坦和傾斜地面的動態(tài)穩(wěn)定行走控制進行研究;文獻[13]針對履帶式移動機器人越障問題,使用質(zhì)心投影法和ZMP對靜態(tài)穩(wěn)定性和動態(tài)穩(wěn)定性分析,得出其越障范圍。
通過國內(nèi)外現(xiàn)狀分析,具有自由度冗余的機器人運動規(guī)劃方法,主要采用優(yōu)化思想;針對考慮安全、防止傾翻的機器人運動控制,主要通過對機器人動力學(xué)穩(wěn)定性進行優(yōu)化來實現(xiàn)。本文研究的搬運機器人,機構(gòu)自由度冗余,搬運對象為傷員,需要避免傾翻、確保穩(wěn)定安全,因此,現(xiàn)提出一種在保證安全性、穩(wěn)定性的前提下搬運機器人搬運傷員的運動規(guī)劃方法,使得傷員的安全得到保證,有效地提高機器人的救援效率。
本文采用基于動力學(xué)穩(wěn)定性優(yōu)化實現(xiàn)搬運傷員運動規(guī)劃和控制的方法。該方法重點保證3個目標:1)搬運過程保持機械臂末端承載傷員的搬運工具運動穩(wěn)定;2)搬運過程保持機器人關(guān)節(jié)空間運動平滑穩(wěn)定;3)保證機器人重心穩(wěn)定,避免傾翻。思路如圖1所示。
圖1 算法思路
運動學(xué)建模包括正運動學(xué)和逆運動學(xué)兩部分,本文重點在于逆運動學(xué),正運動學(xué)是為了進行逆運動學(xué)的準備。
機器人三維模型及坐標系定義如圖2所示。
圖2 搬運機器人坐標系
圖2中:OW為世界坐標系;OR為機器人坐標系;OV為車體坐標系;OA為機械臂坐標系;O1、O2、O3、O4、O5分別為機械臂關(guān)節(jié)一、二、三、四、五坐標系。
本文采用改進DH法進行搬運機器人運動學(xué)建模?;谧鴺讼刀x,改進D-H參數(shù)確定如表1所示。
表1 機器人連桿改進D-H參數(shù)表
其中,關(guān)節(jié)1和3對應(yīng)車體旋轉(zhuǎn)和移動,關(guān)節(jié)5~關(guān)節(jié)9對應(yīng)五自由度機械臂。為滿足MATLAB建模需要,引入虛擬關(guān)節(jié)2和關(guān)節(jié)4。
根據(jù)改進D-H表,末端坐標在世界坐標系中的矩陣跡表達式為
正向運動學(xué)通過MATLAB 仿真試驗得到驗證。仿真試驗?zāi)P图膀炞C試驗關(guān)節(jié)角和機械臂末端笛卡爾坐標如圖3所示。
圖3 MATLAB機器人正向運動學(xué)仿真驗證截圖
假設(shè):機械臂末端坐標系在世界坐標系下的表示為
得
解得:
求得關(guān)節(jié)八的角度為
求得關(guān)節(jié)九的角度為
令:
求得關(guān)節(jié)六的角度為
令:
求得關(guān)節(jié)五的角度為
根據(jù)式(10)得出關(guān)節(jié)七的角度為
根據(jù)作業(yè)過程機械臂時間最短篩選確定關(guān)節(jié)角唯一解。
結(jié)合正向運動學(xué),利用雙向計算方法基于MATLAB驗證了逆向運動學(xué)的有效性。
搬運機器人整體穩(wěn)定性分析包括兩部分:一是動力學(xué)建模;二是動態(tài)穩(wěn)定性建模。
考慮搬運機器人移動操作機構(gòu)復(fù)雜性,本文采用拉格朗日法推導(dǎo)搬運機器人動力學(xué)方程。
3.1.1 求動能K
為求解機器人的動能K,需要確定連桿的線速度和角速度,為此,需要確定雅可比矩陣,具體包括線速度雅可比矩陣和角速度雅可比矩陣。
為獲得線速度雅可比矩陣,計算各連桿i的質(zhì)心在世界坐標系下的表達為
進而線速度雅可比矩陣計算公式為
連桿質(zhì)心線速度為
對于角速度雅可比矩陣計算,旋轉(zhuǎn)關(guān)節(jié)用旋轉(zhuǎn)軸向量表示,移動關(guān)節(jié)用0向量表示,故搬運機器人角速度的雅可比矩陣為
則連桿質(zhì)心角速度為
基于上述公式推導(dǎo)可得動能方程為
機器人的質(zhì)量矩陣為
3.1.2 求勢能U
根據(jù)式(18),求得搬運機器人單個關(guān)節(jié)的勢能:
根據(jù)式(25),求得搬運機器人的整體的勢能為
3.1.3 求動力學(xué)方程
將動能和勢能代入拉格朗日函數(shù)為
最后得到機器人系統(tǒng)的動力學(xué)模型為
3.1.4 正逆動力學(xué)
對于式(28),通過正動力學(xué)求解,已知各關(guān)節(jié)驅(qū)動力矩τ、連桿質(zhì)心規(guī)劃姿態(tài)θ、連桿質(zhì)心規(guī)劃速度θ˙,求解各連桿質(zhì)心的慣性力(即加速度),進行穩(wěn)定性分析。
本文結(jié)合動力學(xué)分析機器人整體動態(tài)穩(wěn)定性。動態(tài)穩(wěn)定性指機器人移動操作過程系統(tǒng)平衡特性,與機器人運動學(xué)模型和動力學(xué)模型有關(guān),本文采用零力矩點(ZMP)方法進行動態(tài)穩(wěn)定性分析。ZMP指某一時刻機器人在支撐面上所有主動力矩之和為0的點,對于整體系統(tǒng)該點只有垂直方向的地面反作用力。搬運機器人保持平衡的一個必要條件是運動過程中ZMP始終落在穩(wěn)定區(qū)域內(nèi)[14]。
通過動力學(xué)計算關(guān)節(jié)轉(zhuǎn)矩和連桿運動關(guān)系,根據(jù)式(28)可得各部位慣性力為
本文研究運動規(guī)劃問題,不考慮不可預(yù)知的外力和外力矩作用;考慮傷員搬運安全性,速度不能過高,為此ZMP簡化為
式中:g為重力加速度,xi、zi為連桿i質(zhì)心z、x方向坐標,為連桿i質(zhì)心沿參考坐標系z、x方向的加速度。
為滿足系統(tǒng)穩(wěn)定性,ZMP點必須在穩(wěn)定區(qū)域之內(nèi),因此穩(wěn)定性的判定依據(jù)為
式中:Xmin為機器人穩(wěn)定區(qū)域的極限最小值,Xmax為機器人穩(wěn)定區(qū)域的極限最大值。
針對機械臂末端點對點的運動規(guī)劃,將傷員從地面移交給運載機器人。常用的速度規(guī)劃方法有S形軌跡規(guī)劃、梯形軌跡規(guī)劃,對于傷員搬運過程,其主要需求在于速度、加速度的連續(xù),與梯型軌跡規(guī)劃方法相比,S形軌跡規(guī)劃時間會增大,但運動更加平穩(wěn),可保證速度、加速度的連續(xù)性,實現(xiàn)末端傷員的安全移動。
為進行S形軌跡規(guī)劃,計算其末端最大速度、最大加速度。根據(jù)雅可比矩陣,建立關(guān)節(jié)額定速度q˙和末端速度的關(guān)系為:
根據(jù)能量守恒,能量不因坐標系不同而改變,結(jié)合式(32)可以得到末端加速度a和關(guān)節(jié)額定力矩τ之間的關(guān)系為
通常S形軌跡規(guī)劃分為7段,分別是加加速段、勻加速段、加減速段、勻速段、減減速段、勻減速段、減加速段,每段都有對應(yīng)的運動學(xué)參數(shù),vti、ati、sti對應(yīng)i段對應(yīng)的速度、加速度、位移,加加速度為j。
在加加速段有:
在勻加速段有:
在加減速段有:
在勻速段有:
在減加速段有:
在勻減速段有:
在減減速段有:
總距離計算公式為
對于S形曲線,按照是否有勻速段、勻變速段可以分為3種情況,上述公式是有勻速段和有勻變速段,另外兩種情況下,計算時需要保證加速度連續(xù)。
S形軌跡規(guī)劃得到機器人末端運動軌跡,這里利用穩(wěn)定性優(yōu)化確定冗余變量機器人移動自由度,進而求解其他關(guān)節(jié)變量。
計算機器人的動態(tài)穩(wěn)定性,根據(jù)式(18)、式(29)得到連桿質(zhì)心坐標xi、zi,以及連桿所受慣性力。
最后,將機器人各部位質(zhì)心坐標和慣性力代入式(30),得到零力矩點在慣性坐標系的表達式,計算ZMP的坐標值。
對于搬運機器人,支撐面是指車體與地面接觸點構(gòu)成的接觸面積。根據(jù)ZMP的坐標值與穩(wěn)定裕度的關(guān)系可以得到最優(yōu)穩(wěn)定性判據(jù)為:
搬運機器人穩(wěn)定性優(yōu)化解析法模型為非線性高耦合方程,為此本文使用數(shù)值解法,計算搬運機器人的最優(yōu)穩(wěn)定性。
圖4表示穩(wěn)定性最優(yōu)時機械臂關(guān)節(jié)一的角度變化過程。由圖4 可以看出,機器人關(guān)節(jié)運動出現(xiàn)明顯不平滑,突變劇烈。
圖4 機械臂關(guān)節(jié)一的角度
對于搬運任務(wù),關(guān)節(jié)出現(xiàn)加速度突變會導(dǎo)致震顫,會導(dǎo)致傷員脫離搬運裝置,或者二次受傷,使傷員安全受到威脅。而且,對于電動機來說,持續(xù)的加速度突變會導(dǎo)致關(guān)節(jié)的使用壽命大幅度縮減,并且會在執(zhí)行救援任務(wù)時,因為機械臂本身能力不足以支持長時間工作而發(fā)生失控現(xiàn)象。為了避免出現(xiàn)這種現(xiàn)象,需要對于關(guān)節(jié)運動進行平滑處理。
本文采用零相位濾波中的巴特沃斯濾波對穩(wěn)定性優(yōu)化結(jié)果進行平滑處理。
首先針對穩(wěn)定性優(yōu)化的車體移動動態(tài)軌跡進行擬合平滑處理,然后利用冗余自由度逆運動學(xué)確定其他關(guān)節(jié)運動軌跡。
本文針對搬運機器人將傷員搬運移交到運載機器人擔架上的過程,設(shè)計末端點的位置坐標,結(jié)合目標傷員搬運機器人在SolidWorks中的自身物理參數(shù)及關(guān)節(jié)數(shù)據(jù),計算穩(wěn)定性對應(yīng)最優(yōu)數(shù)值解,最后求解軌跡中關(guān)節(jié)角度的數(shù)值,并采用巴特沃斯濾波進行平滑處理;利用MATLAB仿真,通過穩(wěn)定性的范圍和運動加速度的平滑性驗證算法有效性。仿真中圖像的周期與時間的轉(zhuǎn)化為T=0.2465 s。
表2 路線對應(yīng)的始末點坐標值m
表3 運動學(xué)參數(shù)
表4 關(guān)節(jié)參數(shù)
表5 質(zhì)量參數(shù)表
表6 Z MP穩(wěn)定區(qū)域坐標值m
表7 巴特沃斯濾波參數(shù)表
1)S形軌跡規(guī)劃仿真。機器人末端沿著世界坐標系z軸方向軌跡規(guī)劃的位移、速度、加速度如圖5~圖7所示。從圖5中可以看出,在z方向上末端按照表2的起始點運動到終點;圖6中速度和圖7中的速度、加速度曲線連續(xù),無突變點,符合S形軌跡規(guī)劃,滿足末端安全需求。
圖5 末端Z方向移動
圖6 末端Z方向移動速度
圖7 末端Z方向移動加速度
2)穩(wěn)定性仿真。機器人整個運動過程中的xzmp如圖8所示,表示機器人自身的穩(wěn)定性,運動過程中,xzmp的數(shù)值為-0.4~0.1 m ,由表6得知,其x方向穩(wěn)定區(qū)域的范圍為-0.7~0.5 m,xzmp的數(shù)值在穩(wěn)定區(qū)域內(nèi)。
圖8 xzmp
3)關(guān)節(jié)變化圖像。根據(jù)冗余自由度逆解得到的平滑擬合后的6個關(guān)節(jié)角度、角速度、角加速度,如圖9~圖20所示,因為末端需要保持人體姿態(tài)不變及移動作業(yè)要求,過程中車體旋轉(zhuǎn)和機械臂關(guān)節(jié)四、關(guān)節(jié)五角度處于相對靜止。
圖9 車體移動距離
圖10 車體移動速度
通過對圖4和圖12進行比較,平滑擬合后的關(guān)節(jié)角度、速度平滑,減少了關(guān)節(jié)角度的頻繁突變。通過圖9~圖20說明平滑擬合后的所有關(guān)節(jié)滿足機器人關(guān)節(jié)連續(xù)運動的要求。
圖12 機械臂一關(guān)節(jié)角度
圖13 機械臂一關(guān)節(jié)角速度
圖14 機械臂一關(guān)節(jié)角加速度
圖15 機械臂二關(guān)節(jié)角度
圖16 機械臂二關(guān)節(jié)角速度
圖17 機械臂二關(guān)節(jié)角加速度
圖18 機械臂三關(guān)節(jié)角度
圖19 機械臂三關(guān)節(jié)角速度
圖20 機械臂三關(guān)節(jié)角加速度
本文針對自由度冗余搬運機器人搬運傷員過程穩(wěn)定性要求,提出對于基于動力學(xué)穩(wěn)定性分析的機器人軌跡規(guī)劃和控制方法,并進行了仿真驗證,主要結(jié)論如下:1)針對自由度冗余搬運機器人,推導(dǎo)出機器人運動學(xué)和動力學(xué)方程;2)結(jié)合S形軌跡規(guī)劃、動態(tài)穩(wěn)定性、求解最優(yōu)解、關(guān)節(jié)角平滑處理,實現(xiàn)了傷員、機器人、關(guān)節(jié)穩(wěn)定的機器人運動規(guī)劃和控制;3)仿真結(jié)果表明,針對搬運機器人,該算法可以完成搬運任務(wù),并能滿足實際任務(wù)安全性、穩(wěn)定性的需求。