安全扭矩約束下的多關(guān)節(jié)機器人軌跡跟隨方法*

張少林，王 穎，王 碩

（1.中國科學(xué)院自動化研究所，北京 100190；2.北方重工集團有限公司，沈陽 110141）

在多關(guān)節(jié)機器人的工作任務(wù)中，不可避免地與外界環(huán)境發(fā)生接觸。為了保護工件、工作平臺，減輕對人員的傷害，常需要對機器人的關(guān)節(jié)扭矩進行約束，同時使機器人保持笛卡爾空間路徑不變，實現(xiàn)設(shè)定的任務(wù)目標。另外，當運動指令的速度和加速度較大時，機器人關(guān)節(jié)電機不能提供所需要的扭矩，會產(chǎn)生沖擊和振動。為了避免機器人的損壞，也需要對關(guān)節(jié)扭矩進行約束。因此，有必要對機器人在安全扭矩約束下的軌跡跟隨方法進行研究。

針對多關(guān)節(jié)機器人的扭矩約束與軌跡規(guī)劃已經(jīng)有很多研究。在很多工業(yè)機器人上，通過在軌跡編程時對最大速度、加速度和加加速度等運動參數(shù)進行約束，使得機器人關(guān)節(jié)扭矩限制在一定范圍之內(nèi)[1]。但是，該方法中的參數(shù)設(shè)置需要依據(jù)經(jīng)驗，

不同的機器人和不同的軌跡需要設(shè)置不同的參數(shù)，不能直觀地設(shè)置扭矩約束。為了直接限制各關(guān)節(jié)的扭矩值，可以在軌跡規(guī)劃和插補完成后，對電機扭矩指令加以限幅，防止扭矩過大[2]。但是這類方法只在控制系統(tǒng)輸出時將扭矩約束到制定范圍內(nèi)，沒有同時考慮扭矩約束與軌跡規(guī)劃。Zanchettin 等[3–4]將軌跡生成技術(shù)和優(yōu)化控制策略合并到一個運動規(guī)劃與控制的框架中，自主地分析程序指令給定的約束條件，實時地轉(zhuǎn)換為運動命令，提高任務(wù)執(zhí)行的可靠性。Yue 等[5]分析了扭矩約束時空間機器人控制系統(tǒng)的穩(wěn)定控制策略。Valente 等[6]提出了一種在運動約束范圍內(nèi)生成平滑軌跡的方法，在裝配過程的運動精度和速度之間達到平衡。這些方法在機器人的工作空間內(nèi)搜索了滿足穩(wěn)定性、時間最優(yōu)等控制性能的軌跡，但是不能針對笛卡爾路徑主動地將扭矩約束到特定范圍內(nèi)。目前，一種常見的扭矩約束與軌跡規(guī)劃方法應(yīng)用在軌跡的最短時間規(guī)劃方面[7–11]。該方法基于動力學(xué)模型分析關(guān)節(jié)最大扭矩限制，通過使至少一個關(guān)節(jié)的驅(qū)動扭矩達到最大來實現(xiàn)運動時間的最小化。通常，該方法先根據(jù)動力學(xué)模型計算各位置點對應(yīng)的最大速度約束，形成位置–速度相平面的約束邊界線。然后從起始點分析最大加速度對應(yīng)的相平面曲線，從終點分析最大減速度對應(yīng)的相平面曲線，并在邊界線的極小值處求相切曲線，從而實現(xiàn)從起始點到終點的最短時間規(guī)劃。另外，郭清達等[12–13]通過遺傳算法計算了機器人在扭矩約束的情況下能夠達到的關(guān)節(jié)速度、加速度和加加速度，從而實現(xiàn)在扭矩限制范圍內(nèi)的軌跡規(guī)劃。但是，這類方法需要針對整條軌跡進行綜合分析，計算量大，耗時較長，通常在計算完成后才開始運動。劉華山等[14–15]通過在控制過程中引入具有飽和特性的正反切函數(shù)來確保扭矩的有界性，并通過模糊自適應(yīng)PD控制來減小超調(diào)量，提升軌跡跟隨性能。但是，飽和函數(shù)的非線性特性可能對系統(tǒng)產(chǎn)生影響，不同函數(shù)的選取需要調(diào)節(jié)不同的控制系統(tǒng)參數(shù)。Dahl[16]通過對軌跡進行時間縮放來實現(xiàn)扭矩約束時的路徑跟蹤。該方法在原控制器基礎(chǔ)上擴展了輔助控制器，控制系統(tǒng)較為復(fù)雜。因此，在保持多關(guān)節(jié)機器人的笛卡爾路徑不變的同時，如何實時施加扭矩約束并保證快速軌跡跟隨還需要進一步研究。

本文將對安全扭矩約束下的多關(guān)節(jié)機器人軌跡跟隨方法進行分析，將機器人各關(guān)節(jié)的扭矩約束到指定范圍內(nèi)，繼續(xù)按照笛卡爾空間路徑運動，并使得機器人能夠快速跟隨指令軌跡，避免運動太慢引起的效率低下。該方法可以提升作業(yè)的安全性，達到保護機器人和作業(yè)環(huán)境的目的。具體將從以下幾點進行分析：

（1）分析機器人軌跡上的動力學(xué)約束，依據(jù)設(shè)定的關(guān)節(jié)扭矩，重新計算最大加速度，使得機器人軌跡跟隨不超過安全扭矩。

（2）在安全扭矩的約束下，實現(xiàn)對指令軌跡的動態(tài)跟隨，達到實時計算與快速跟隨的目的。

扭矩限制下的軌跡跟隨

分析多關(guān)節(jié)機器人的受力為：

其中，Mq是質(zhì)量矩陣，和是離心力和哥氏力，G 是重力，fτ是摩擦力，是關(guān)節(jié)慣性力，是電機提供的驅(qū)動扭矩是限制扭矩，q、是關(guān)節(jié)的角度值、角速度和角加速度。q 由關(guān)節(jié)位置傳感器測量得到，通過對q 微分和濾波得到。在扭矩約束下，當機械臂的關(guān)節(jié)運動到位置q 時，和需要滿足上式，以保證扭矩不超過限制值。

設(shè)機器人軌跡規(guī)劃后，得到的運動曲線為s(t)∈R（t表示時間）。當關(guān)節(jié)扭矩被限制，不足以跟隨指令軌跡時，應(yīng)當保持路徑不變，修改關(guān)節(jié)速度和加速度來滿足扭矩要求。關(guān)節(jié)空間的位置、角速度和角加速度與曲線s(t)之間的關(guān)系如下：

式中，h變換可以表示為h(s)= fInvk[fPos(s)]。fPos(s)∈R6為將長度s 映射到指令軌跡的笛卡爾坐標點，fInvk表示求運動學(xué)逆運算。

設(shè)曲線映射關(guān)系fPos(s)為

式中，P0為起始點，n為與長度s對應(yīng)的曲線單位矢量。考慮到q=fInvk(P)，求導(dǎo)可得：

式中，Jb為雅克比矩陣。因此，由式（4）和式（5）可得：

由式（3）和式（6）可以推導(dǎo)h的導(dǎo)數(shù)為：

式中，M=Mq+Mj，和分別表示笛卡爾空間的速度和加速度。由式（9）可以得到和的約束關(guān)系為：

步驟1：設(shè)當前機器人狀態(tài)為sc和。依據(jù)S型加減速[17]計算下一個周期的指令軌跡點sd、和。

步驟2：采用PID和前饋控制[18]計算下一個周期的笛卡爾空間位置、速度和加速度：

步驟4：比較τtotal和τmax的大小。如果則扭矩在限制范圍內(nèi)，跳轉(zhuǎn)步驟6。否則，跳轉(zhuǎn)步驟5計算扭矩約束。

步驟5：比較Rτtotal和Rτmax，判斷是第幾關(guān)節(jié)扭矩超限。依據(jù)式（10）求解此時的最大加速度s˙˙并積分得到和s。

圖1 扭矩約束方法計算流程Fig.1 Calculation flowchart of torque constraint method

跳轉(zhuǎn)步驟3繼續(xù)執(zhí)行。

步驟7：等待下一個計算周期的到來，跳轉(zhuǎn)步驟1，依次循環(huán)，直到軌跡運行結(jié)束。

試驗與分析

1 試驗平臺

為了對所述方法進行驗證，在如圖2所示機械臂上進行了試驗。該機械具有6個自由度，全部都是旋轉(zhuǎn)關(guān)節(jié)。機械臂上選用了較大功率的電機，并將所有關(guān)節(jié)的驅(qū)動電機都布置在底座上，通過中間傳動件驅(qū)動末端關(guān)節(jié)運動。為了減小連桿慣量，設(shè)計了緊湊的傳動方式。該機械臂的技術(shù)參數(shù)如表1所示。機器人關(guān)節(jié)臂最大長度約為1m。6個電機的功率都是200W。不考慮基座時，機械臂重量為13.8kg，且重量都集中在底座附近。末端執(zhí)行器最大速度超過8m/s。各關(guān)節(jié)的運動角度、額定轉(zhuǎn)矩和最大轉(zhuǎn)矩如表2所示。該機械臂為人機協(xié)作臂，為了提高協(xié)作時的安全性，需要對機械臂各關(guān)節(jié)扭矩進行限制。

表1 機械臂技術(shù)參數(shù)Table1 Technical parameters of manipulator

表2 機械臂各關(guān)節(jié)參數(shù)Table2 Joint parameters of manipulator

2 扭矩約束試驗

為了驗證機械臂是否在限定的扭矩范圍內(nèi)跟隨指令軌跡運動，首先不限制關(guān)節(jié)扭矩，將如圖3所示的笛卡爾空間直線段設(shè)置為指令軌跡，使機器人沿該軌跡運動。該軌跡根據(jù)S型加減速算法計算速度和加速。當機器人沿該軌跡運動時，采樣電機電流并變換得到關(guān)節(jié)驅(qū)動扭矩，如圖4所示。

圖2 操作臂實物圖Fig.2 Picture of manipulator

為驗證扭矩約束方法的可行性，現(xiàn)以14N·m為例（即τmax=14N·m），將各關(guān)節(jié)扭矩約束到該扭矩范圍之內(nèi)，使機器人繼續(xù)沿該軌跡運動并重新規(guī)劃。圖5為重新規(guī)劃后的關(guān)節(jié)扭矩，可見，圖4中扭矩1 超過14N·m的部分在圖5中得到了約束，且在約束部分逼近14N·m。同時，為了保持路徑不變，其他關(guān)節(jié)扭矩也相應(yīng)變化。根據(jù)Chen 等[7–8]研究，當有關(guān)節(jié)的扭矩達到最大值時，其他關(guān)節(jié)的扭矩在最大值范圍內(nèi)相應(yīng)調(diào)節(jié)可以使得機器人跟隨指令軌跡，這種情況下沿著指令軌跡的運動時間是最短的。這種情況對應(yīng)了扭矩約束時的最大加速度，具有最快的跟隨效果。

重新規(guī)劃的軌跡是基于運動曲線參數(shù)s進行的。該方法在原直線路徑的基礎(chǔ)上改變速度和加速度來實現(xiàn)扭矩約束，不改變原笛卡爾空間路徑。因此，重新規(guī)劃后仍然是如圖3所示的直線段。圖6為扭矩約束前后運動曲線參數(shù)s的對比（其中，藍色虛線為扭矩約束后的s，紅色實線為扭矩約束前的s）。虛線軌跡在N=100左右偏離了實線軌跡，這說明重新規(guī)劃后速度和加速度發(fā)生了變化，與指令軌跡存在偏差。但是，虛線軌跡隨后又快速跟隨了實線的變化，這說明扭矩約束解除后，重新規(guī)劃的軌跡快速跟隨了指令軌跡。

圖3 機械臂指令軌跡Fig.3 Command trajectory of manipulator

圖4 指令軌跡對應(yīng)的關(guān)節(jié)扭矩Fig.4 Joint torques corresponding to command trajectory

結(jié)論

本文給出了一種多關(guān)節(jié)機器人在安全扭矩約束下的軌跡跟隨方法。該方法依據(jù)動力學(xué)方程約束，通過空間曲線參數(shù)s來保證笛卡爾空間路徑不變，重新計算了當前允許的最大加速度，確保軌跡跟隨過程不超過安全扭矩限制，提高了機械臂與人協(xié)作的安全性。同時，在安全扭矩的約束下，實現(xiàn)對指令軌跡的動態(tài)跟隨，達到實時計算與快速跟隨的目的。在六自由度自研輕量化協(xié)作機械臂上的試驗驗證了該方法的可行性。

圖6 指令軌跡與跟隨軌跡Fig.6 Command trajectory and following trajectory