基于動力學的搬運機器人運動規劃控制算法與仿真

滿文鏢，李艷杰，卜春光，高英麗，郎智明，眭晉

（1.沈陽理工大學機械工程學院，沈陽 110016; 2.中國科學院沈陽自動化研究所，沈陽 110016）

0 引言

面向危樓傷員救援需要，基于移動操作協調技術路線，我團隊研制出傷員搬運機器人和運載機器人。搬運機器人負責將傷員從地面抬起并在合適的位置、高度移交給運載機器人，搬運機器人本體由2自由度履帶式移動機構、5自由度機械臂和傷員搬運工具構成，機器人運動自由度數總計為7個，運動自由度存在冗余。機器人搬運對象為傷員，為了提高搬運過程傷員的安全性、舒適性，如何規劃機器人搬運運動，確定自由度冗余情況下搬運機器人運動學逆解，控制機器人保持傷員、機器人重心穩定，避免傾翻危險，是搬運機器人需要解決的關鍵核心問題。

關于冗余自由度機器人不同需求運動規劃的應用，通過動力學模型的建立，文獻[1]、[2]分別提出時間最優和能耗最優的軌跡規劃方法；為實現實時軌跡規劃，文獻[3]基于雅可比矩陣提出了人工勢場軌跡規劃方法，文獻[4]利用虛擬排斥速度場和機械臂自運動特性實現了避障的運動規劃；文獻[5]考慮避障環境為封閉類管道下進行零件噴涂，提出基于碰撞反饋的冗余機械臂避障算法；文獻[6]通過改進麻雀搜索法對超冗余機械臂進行軌跡規劃；針對雙足多自由度機器人，文獻[7]實現18自由度雙足機器人的全身運動規劃，文獻[8]、[9]考慮傾斜地面，針對多自由度雙足機器人行走的最優軌跡進行運動規劃。

針對傾翻問題，文獻[10]針對兩足機器人的運動穩定性進行分析，文獻[11]針對兩足機器人接觸點的平衡穩定性進行研究，文獻[12]針對兩足類人生物在不平坦和傾斜地面的動態穩定行走控制進行研究；文獻[13]針對履帶式移動機器人越障問題，使用質心投影法和ZMP對靜態穩定性和動態穩定性分析，得出其越障范圍。

通過國內外現狀分析，具有自由度冗余的機器人運動規劃方法，主要采用優化思想；針對考慮安全、防止傾翻的機器人運動控制，主要通過對機器人動力學穩定性進行優化來實現。本文研究的搬運機器人，機構自由度冗余，搬運對象為傷員，需要避免傾翻、確保穩定安全，因此，現提出一種在保證安全性、穩定性的前提下搬運機器人搬運傷員的運動規劃方法，使得傷員的安全得到保證，有效地提高機器人的救援效率。

1 算法思路

本文采用基于動力學穩定性優化實現搬運傷員運動規劃和控制的方法。該方法重點保證3個目標：1）搬運過程保持機械臂末端承載傷員的搬運工具運動穩定；2）搬運過程保持機器人關節空間運動平滑穩定；3）保證機器人重心穩定，避免傾翻。思路如圖1所示。

圖1 算法思路

2 搬運機器人運動學建模

運動學建模包括正運動學和逆運動學兩部分，本文重點在于逆運動學，正運動學是為了進行逆運動學的準備。

2.1 搬運機器人正向運動學

機器人三維模型及坐標系定義如圖2所示。

圖2 搬運機器人坐標系

圖2中：OW為世界坐標系；OR為機器人坐標系；OV為車體坐標系；OA為機械臂坐標系；O1、O2、O3、O4、O5分別為機械臂關節一、二、三、四、五坐標系。

本文采用改進DH法進行搬運機器人運動學建模?；谧鴺讼刀x，改進D-H參數確定如表1所示。

表1 機器人連桿改進D-H參數表

其中，關節1和3對應車體旋轉和移動，關節5～關節9對應五自由度機械臂。為滿足MATLAB建模需要，引入虛擬關節2和關節4。

根據改進D-H表，末端坐標在世界坐標系中的矩陣跡表達式為

正向運動學通過MATLAB 仿真試驗得到驗證。仿真試驗模型及驗證試驗關節角和機械臂末端笛卡爾坐標如圖3所示。

圖3 MATLAB機器人正向運動學仿真驗證截圖

2.2 搬運機器人逆向運動學

假設：機械臂末端坐標系在世界坐標系下的表示為

得

解得：

求得關節八的角度為

求得關節九的角度為

令：

求得關節六的角度為

令：

求得關節五的角度為

根據式（10）得出關節七的角度為

根據作業過程機械臂時間最短篩選確定關節角唯一解。

結合正向運動學，利用雙向計算方法基于MATLAB驗證了逆向運動學的有效性。

3 搬運機器人穩定性建模

搬運機器人整體穩定性分析包括兩部分：一是動力學建模；二是動態穩定性建模。

3.1 動力學建模

考慮搬運機器人移動操作機構復雜性，本文采用拉格朗日法推導搬運機器人動力學方程。

3.1.1 求動能K

為求解機器人的動能K，需要確定連桿的線速度和角速度，為此，需要確定雅可比矩陣，具體包括線速度雅可比矩陣和角速度雅可比矩陣。

為獲得線速度雅可比矩陣，計算各連桿i的質心在世界坐標系下的表達為

進而線速度雅可比矩陣計算公式為

連桿質心線速度為

對于角速度雅可比矩陣計算，旋轉關節用旋轉軸向量表示，移動關節用0向量表示，故搬運機器人角速度的雅可比矩陣為

則連桿質心角速度為

基于上述公式推導可得動能方程為

機器人的質量矩陣為

3.1.2 求勢能U

根據式（18），求得搬運機器人單個關節的勢能：

根據式（25），求得搬運機器人的整體的勢能為

3.1.3 求動力學方程

將動能和勢能代入拉格朗日函數為

最后得到機器人系統的動力學模型為

3.1.4 正逆動力學

對于式（28），通過正動力學求解，已知各關節驅動力矩τ、連桿質心規劃姿態θ、連桿質心規劃速度θ˙，求解各連桿質心的慣性力（即加速度），進行穩定性分析。

3.2 搬運機器人穩定性分析

本文結合動力學分析機器人整體動態穩定性。動態穩定性指機器人移動操作過程系統平衡特性，與機器人運動學模型和動力學模型有關，本文采用零力矩點（ZMP）方法進行動態穩定性分析。ZMP指某一時刻機器人在支撐面上所有主動力矩之和為0的點，對于整體系統該點只有垂直方向的地面反作用力。搬運機器人保持平衡的一個必要條件是運動過程中ZMP始終落在穩定區域內[14]。

通過動力學計算關節轉矩和連桿運動關系，根據式（28）可得各部位慣性力為

本文研究運動規劃問題，不考慮不可預知的外力和外力矩作用；考慮傷員搬運安全性，速度不能過高，為此ZMP簡化為

式中：g為重力加速度，xi、zi為連桿i質心z、x方向坐標，為連桿i質心沿參考坐標系z、x方向的加速度。

為滿足系統穩定性，ZMP點必須在穩定區域之內，因此穩定性的判定依據為

式中：Xmin為機器人穩定區域的極限最小值，Xmax為機器人穩定區域的極限最大值。

4 S形軌跡規劃

針對機械臂末端點對點的運動規劃，將傷員從地面移交給運載機器人。常用的速度規劃方法有S形軌跡規劃、梯形軌跡規劃，對于傷員搬運過程，其主要需求在于速度、加速度的連續，與梯型軌跡規劃方法相比，S形軌跡規劃時間會增大，但運動更加平穩，可保證速度、加速度的連續性，實現末端傷員的安全移動。

為進行S形軌跡規劃，計算其末端最大速度、最大加速度。根據雅可比矩陣，建立關節額定速度q˙和末端速度的關系為：

根據能量守恒，能量不因坐標系不同而改變，結合式（32）可以得到末端加速度a和關節額定力矩τ之間的關系為

通常S形軌跡規劃分為7段，分別是加加速段、勻加速段、加減速段、勻速段、減減速段、勻減速段、減加速段，每段都有對應的運動學參數，vti、ati、sti對應i段對應的速度、加速度、位移，加加速度為j。

在加加速段有：

在勻加速段有：

在加減速段有：

在勻速段有：

在減加速段有：

在勻減速段有：

在減減速段有：

總距離計算公式為

對于S形曲線，按照是否有勻速段、勻變速段可以分為3種情況，上述公式是有勻速段和有勻變速段，另外兩種情況下，計算時需要保證加速度連續。

5 基于穩定性優化的冗余自由度機器人逆運動學解確定

S形軌跡規劃得到機器人末端運動軌跡，這里利用穩定性優化確定冗余變量機器人移動自由度，進而求解其他關節變量。

計算機器人的動態穩定性，根據式（18）、式（29）得到連桿質心坐標xi、zi，以及連桿所受慣性力。

最后，將機器人各部位質心坐標和慣性力代入式（30），得到零力矩點在慣性坐標系的表達式，計算ZMP的坐標值。

對于搬運機器人，支撐面是指車體與地面接觸點構成的接觸面積。根據ZMP的坐標值與穩定裕度的關系可以得到最優穩定性判據為：

搬運機器人穩定性優化解析法模型為非線性高耦合方程，為此本文使用數值解法，計算搬運機器人的最優穩定性。

圖4表示穩定性最優時機械臂關節一的角度變化過程。由圖4 可以看出，機器人關節運動出現明顯不平滑，突變劇烈。

圖4 機械臂關節一的角度

對于搬運任務，關節出現加速度突變會導致震顫，會導致傷員脫離搬運裝置，或者二次受傷，使傷員安全受到威脅。而且，對于電動機來說，持續的加速度突變會導致關節的使用壽命大幅度縮減，并且會在執行救援任務時，因為機械臂本身能力不足以支持長時間工作而發生失控現象。為了避免出現這種現象，需要對于關節運動進行平滑處理。

本文采用零相位濾波中的巴特沃斯濾波對穩定性優化結果進行平滑處理。

首先針對穩定性優化的車體移動動態軌跡進行擬合平滑處理，然后利用冗余自由度逆運動學確定其他關節運動軌跡。

6 仿真驗證

本文針對搬運機器人將傷員搬運移交到運載機器人擔架上的過程，設計末端點的位置坐標，結合目標傷員搬運機器人在SolidWorks中的自身物理參數及關節數據，計算穩定性對應最優數值解，最后求解軌跡中關節角度的數值，并采用巴特沃斯濾波進行平滑處理；利用MATLAB仿真，通過穩定性的范圍和運動加速度的平滑性驗證算法有效性。仿真中圖像的周期與時間的轉化為T=0.2465 s。

6.1 仿真輸入數據（如表2～表6）

表2 路線對應的始末點坐標值m

表3 運動學參數

表4 關節參數

表5 質量參數表

表6 Z MP穩定區域坐標值m

表7 巴特沃斯濾波參數表

6.2 仿真結果

1）S形軌跡規劃仿真。機器人末端沿著世界坐標系z軸方向軌跡規劃的位移、速度、加速度如圖5～圖7所示。從圖5中可以看出，在z方向上末端按照表2的起始點運動到終點；圖6中速度和圖7中的速度、加速度曲線連續，無突變點，符合S形軌跡規劃，滿足末端安全需求。

圖5 末端Z方向移動

圖6 末端Z方向移動速度

圖7 末端Z方向移動加速度

2）穩定性仿真。機器人整個運動過程中的xzmp如圖8所示，表示機器人自身的穩定性，運動過程中，xzmp的數值為-0.4～0.1 m ，由表6得知，其x方向穩定區域的范圍為-0.7～0.5 m，xzmp的數值在穩定區域內。

圖8 xzmp

3）關節變化圖像。根據冗余自由度逆解得到的平滑擬合后的6個關節角度、角速度、角加速度，如圖9～圖20所示，因為末端需要保持人體姿態不變及移動作業要求，過程中車體旋轉和機械臂關節四、關節五角度處于相對靜止。

圖9 車體移動距離

圖10 車體移動速度

通過對圖4和圖12進行比較，平滑擬合后的關節角度、速度平滑，減少了關節角度的頻繁突變。通過圖9～圖20說明平滑擬合后的所有關節滿足機器人關節連續運動的要求。

圖12 機械臂一關節角度

圖13 機械臂一關節角速度

圖14 機械臂一關節角加速度

圖15 機械臂二關節角度

圖16 機械臂二關節角速度

圖17 機械臂二關節角加速度

圖18 機械臂三關節角度

圖19 機械臂三關節角速度

圖20 機械臂三關節角加速度

7 結論

本文針對自由度冗余搬運機器人搬運傷員過程穩定性要求，提出對于基于動力學穩定性分析的機器人軌跡規劃和控制方法，并進行了仿真驗證，主要結論如下：1）針對自由度冗余搬運機器人，推導出機器人運動學和動力學方程；2）結合S形軌跡規劃、動態穩定性、求解最優解、關節角平滑處理，實現了傷員、機器人、關節穩定的機器人運動規劃和控制；3）仿真結果表明，針對搬運機器人，該算法可以完成搬運任務，并能滿足實際任務安全性、穩定性的需求。