基于自適應radau偽譜法的機械手軌跡規劃方法研究

王 帥，楊 冬，陳貴亮，唐潤智

（河北工業大學 機械工程學院，天津 300132）

0 引言

目前，隨著社會發展，機械手已經得到廣泛應用。但機械手在面對復雜的作業任務時，經常存在著能量消耗大、負載自重比低等問題，如何實現機械手的軌跡優化，降低機械手的關節力矩，提升機械手的負載能力是一個值得研究的問題。近年來已經有很多學者對這方面進行了研究。洪磊等人運用最小范數廣義力策略優化協調操作臂載荷分配，提出了改進的三分支機器人優化關節力矩軌跡規劃方法[1]，但是犧牲了一部分能量消耗指標。張威等人在速度水平上對冗余機器人利用冗余特性進行了動力學優化[2]，但沒有在加速度水平上進行優化。金波等人提出了以降低系統能耗為目標的六足機器人力矩分配算法[3]，但這種算法不一定適合于普通的機械手。邵浩然等人采用遺傳算法對機械臂的各關節力矩進行了優化[4]，陳珂等人采用粒子群算法對并聯機器人的輸出力矩進行優化[5]，但是對于串聯機器人并不適用。

偽譜法[6]是將連續時間最優控制問題轉化為有限維的非線性規劃（NLP）問題進行有效求解，首先設置一系列的配點，將控制變量和狀態變量進行離散，然后在配點上進行多項式插值來逼近狀態變量和控制變量進行優化，而自適應radau偽譜法[7～9]是偽譜法的一種，將整個運動過程進行分段，可以根據計算過程中的精度要求，動態調整段的個數和多項式的階數來提高求解速率，有著收斂速度快、對初值要求不敏感等特點。江浩斌等人采用自適應radau偽譜法解決了智能汽車的軌跡規劃問題[10]，但是應用場景單一且并沒有用于機器人。

為了證明所用方法對于機械手的普適性，且在提高機械手的負載能力方面具有優勢，本文以二自由度和三自由度機械手為對象，采用自適應radau偽譜法進行軌跡優化，相比于其他算法，此方法不需要提前對機械手的軌跡進行參數化擬合。首先建立機械手的動力學模型，并以關節力矩平方和為目標函數在多約束條件下利用該種方法將最優控制問題轉為線性規劃問題進行求解。與五次多項式軌跡規劃相比，此方法降低了機械手的關節力矩，提高機械手的負載能力。

1 動力學建模與Bolza型最優控制描述

利用拉格朗日方程可得機械臂的動力學模型為：

q為機械臂的關節位移，q為機械臂的關節速度，u為關節力矩，將動力學方程轉化為狀態方程，令x為狀態向量，u為控制向量，x1=q，x2=q，x=[x1，x2]，建立狀態方程如下：

將動力學方程轉為狀態方程可得：

其中M為機械臂的慣性矩陣，C為離心力和科氏力矢量，G為重力矢量，t為時間。

1.1 二自由度機械手動力學模型

在機械手末端懸掛4kg重物，二自由度機械手經簡化后建立如圖1所示坐標系：

圖1 二自由度機械手簡化模型

將式(1)展開，利用拉格朗日方程建立機械手的動力學方程為：

其中：

式(5)中各符號代表含義及其機械手的參數如表1所示：

表1 二自由度機械手參數

1.2 三自由度機械手動力學模型

三自由度機械手經簡化后建立如圖3所示坐標系：

圖3 三自由度機械臂簡化模型

利用拉格朗日方程推導將式(1)展開可得三自由度機械手的動力學方程[11]為：

其中：

相對于二自由度機械手而言，主要是增加了旋轉臂的部分，旋轉臂部分的參數表如表2所示：

表2 旋轉臂部分參數

1.3 Bolza型最優控制模型

Bolza型最優控制問題[12]可定義如下：

約束條件為：

式(9)中，t0表示起始時間，tf表示終止時間，約束條件中x表示狀態約束，C表示路徑約束，φ表示邊界約束。就本研究的機械手而言，狀態約束即為對機械手各關節的位置和速度的起始、終止狀態約束，邊界約束即限制位置、速度、關節力矩的邊界條件，假設空間內無障礙物，所以無路徑約束。機械手的狀態約束及邊界約束參數如表3所示，其中角度單位為rad，速度單位為rad/s，關節力矩單位為N·m。

表3 機械手約束

本文以機械手關節力矩的平方和最小為優化目標即：

式(10)中ui(t)表示機械臂第i個關節在t時刻的關節力矩。n表示機械臂的總關節數。

2 自適應radau偽譜法

偽譜法一般用于飛行器的軌跡優化問題，本文采用自適應radau偽譜法進行關節型機械手的軌跡優化。偽譜法是將連續的最優控制問題離散化為非線性規劃問題，以拉格朗日多項式在離散點之間逼近狀態變量和控制變量，求得控制變量和狀態變量最優解[13]；在解決復雜問題時，約束條件也在增加，最優控制問題的求解速度也在不斷下降，而自適應則是在求解過程中動態調整網格區間個數和多項式階數來實現快速收斂[14]。相比于其他方法，自適應radau偽譜法不需要提前對軌跡進行插值擬合，存在著對狀態變量和控制變量求解精度高、收斂速度快的特點。

2.1 radau偽譜法

自適應radau偽譜法首先將最優控制問題的時間t∈[t0,tf]通過變換映射到偽譜法所要求的時間區間[-1，1]上，變換方程為：

N次Legendre多項式RN(τ)的表達式為：

其中N個Legendre-Gauss-Radau（LGR）配點(τ1,τ2,...τN）為N 次Legendre多項式PN(τ)的零點，其中τ1=-1,τN+1=1,τN＜ 1。

拉格朗日基函數為：

然后應用N階拉格朗日多項式對最優控制問題的狀態變量和控制變量進行類似：

其中x為狀態變量，X為近似狀態變量，u為控制變量，U為近似控制變量。

對式(14)求微分，可得狀態變量對時間的導數為：

其中：

其中：

g(τl)=(1+τl)[PN（τl）-PN-1（τl）]，ll=1,2...N+1為N次Legendre多項式PN(τ)的零點和一個終端點。

由式(16)將狀態方程進行離散可得：

本研究所用的機械手是以關節力矩平方和作為目標函數，目標函數經過離散得：

2.2 自適應原則

首先考慮時間間隔t∈[t0,tf]上的Bolza最優控制問題，將整個時間間隔分成k段，每段的間隔為[t k-1,tk]，Nk為每段中配點的個數，自適應算法的目標是通過確定當前網格的特定段是否已滿足指定的精度公差，以計算有效的方式提高解的精度。如果未達到精度公差，則需要通過增加每段中多項式的階數或進一步劃分段來修改配點的數量和分布[15]。

若通過多項式階數來更新，則Nk-和Nk+為更新前后每個時間段內的配點數，其與誤差容忍度εd和在每一時間段內通過設定離散狀態方程和路徑約束的最大誤差值e(k)max的關系表達式為：

其中X為任意整數常量，控制每一段內配點數的增長數，而ceil是舍入到下一個較大整數的運算符。

若通過進一步劃分段來修改配點的數量和分布則子區間段的個數可表示為：

其中Y為任意的整數常數，來控制子區間段的個數。而ceil是將參數舍入為+∞的運算符。

3 系統仿真

3.1 仿真設定

針對機械手的負載能力提升問題，在軌跡規劃時以降低機械手的關節力矩為目標，以二自由度機械手和三自由度機械手模型為例，以關節位置、速度作為狀態變量，關節力矩作為控制變量，采用自適應radau偽譜法對機械手進行軌跡優化，首先設置機械手的狀態約束和邊界約束，具體參數如表3所示，然后利用已建立的機械手動力學模型進行關節力矩的求解，代入以關節力矩平方和作為優化目標函數，利用MATLAB外接的gpops-Ⅱ[16]軟件進行求解。建立機械手的五次多項式軌跡規劃，并與自適應radau偽譜法進行對比，驗證優化方法的有效性，仿真時長設置為1s。

3.2 仿真結果

圖4(a)、圖4(b)和圖4(c)分別為二自由度機械手經過五次多項式規劃的的關節位置、速度和力矩圖像，圖4(d)、圖4(e)和圖4(f)分別為三自由度機械手經過五次多項式規劃的關節位置、速度和力矩圖像。由圖像可以看出，各關節在達到設定的位置時，關節力矩呈現升高-降低-再升高的趨勢，時間在0.2s時關節力矩達到峰值。

圖4 機械手五次多項式軌跡規劃

圖5(a)、圖5(b)為二自由度機械手經過偽譜法和五次多項式關節力矩對比。圖5(c)、圖5(d)為優化之后機械手的關節位置、速度圖像。由圖可知，在整個運動過程中，經過優化之后大臂力矩相對于優化前呈整體降低的趨勢，小臂力矩在初始時有一個較高值，但隨著機械臂的運動，不斷低于優化前，且總體呈平穩趨勢。圖6(a)、圖6(b)和圖6(c)分別為三自由度機械手經過偽譜法和五次多項式關節力矩對比，圖6(d)、圖6(e)為優化之后機械手的關節位置、速度圖像。由圖像可以看出，優化后旋轉臂關節力矩相比于優化前呈平穩下降趨勢，大臂關節力矩在0.3s左右達到峰值，且比優化前幅值下降了約為10N·m，之后平穩下降。小臂的關節位置、速度相比于其他兩個關節有一個較大的浮動，關節力矩優化后在初始時刻即為峰值，且為優化前的四倍，但隨著時間進行，逐漸降低，從全局來看，總體還是呈下降趨勢。

圖5 二自由度機械手偽譜法與五次多項式關節力矩對比

圖6 三自由度機械手偽譜法與五次多項關節力矩對比

對于機械手模型，在整個的仿真過程中，小臂關節在初始時刻要大于優化前的關節力矩，但隨著仿真的運行，關節力矩逐漸低于優化前，機械手其余各關節力矩相比于優化前呈總體降低的趨勢，從而提高了機械手的負載能力，也間接地降低了能耗，證明了所采用方法的合理性、有效性。

4 實驗

實驗器材為自主研制的HEBUT-Ⅱ型三自由度機械手，旋轉臂的傳動比為120，大臂的傳動比為100，小臂的傳動比為80，各臂質量、長度等參數如表1、表2所示，電機均為安川電機，額定扭矩為1.27N·m。型號為SGM7J-04A，控制系統由凌華PCI-8258DSP型8軸高級運動控制卡和工控機組成，采用Labview作為上位機開發軟件進行實驗。

由于位置控制時受位置精度的影響，電機的扭矩輸出無規律性，與仿真相差較大，故實驗采用力矩控制模式，通過Labview編程分別將經過自適應radau偽譜法和五次多項式規劃的關節力矩經過換算轉為輸入電壓，輸入到伺服驅動器以驅動電機進行關節的轉動，將伺服驅動器的模擬量輸出CN5端口與數據采集卡的模擬量輸入端口相連，讀取反饋回來的電壓值，然后將電壓值經過換算轉為力矩值，作為我們的實驗力矩輸出值。實驗場景如圖7所示。

圖7 實驗場景

實驗得到的數據如圖8所示，圖8(a)、圖8(b)為二自由度機械手實驗后的關節力矩對比圖像，圖8(c)、圖8(d)和圖8(e)分別為三自由度機械手實驗后的關節力矩對比圖像。在力矩控制模式下，機械手基本上能達到各關節位置，對于二自由度機械手，優化后的關節力矩在初始時刻有所降低，與仿真時相差不大，大臂關節力矩在最后時刻趨于同一值，總體呈平穩下降趨勢，產生的關節力矩較小。對于三自由度機械手，在運行過程中在某一時刻有尖點的存在，是由于伺服系統反應速度（高）與機械傳遞或者反應時間（較長）不相匹配而引起的，旋轉臂關節一直呈平穩下降趨勢，大臂關節的峰值依然小于優化前，但峰值出現在0.4s左右，相比于仿真峰值出現時間延后了0.2s，小臂關節的力矩呈現降低-升高-再降低-再升高的趨勢，且在0.5s時刻關節力矩大于優化前，這可能是由于機械手各關節本身存在的摩擦，機械臂為非均質桿導致動力學模型不準確的原因，但從總體來看，實驗所得關節力矩的圖像與仿真基本上趨勢相同，優化之后的關節力矩相比優化前峰值明顯降低，也就是提高了負載能力。

圖8 關節力矩對比實驗

5 結語

本文以降低機械手的關節力矩，提升負載能力為目標，采用自適應radau偽譜法對機械手進行軌跡優化。根據二自由度和三自由度的機械手動力學模型，以位置、速度作為狀態變量，力矩作為控制變量并考慮運動過程中的狀態約束、邊界約束，以關節力矩平方和作為優化目標函數，將運動過程中的連續最優控制問題轉化為非線性規劃問題。此種算法在求解過程中可動態調整運動過程分段個數和多項式階數來提高求解速率。由仿真可得優化后的機械手在達到關節位置后，其關節力矩相比于優化前都有了很大程度的降低，即在不更換電機及驅動器的前提下，給它同等大小的力矩可以提升更重的物體，也就是提高了機械手的負載能力。也間接降低了機械手的能耗，并通過實驗驗證了此種方法的合理性，有效性。本文并沒有再對更多自由度的機械手進行研究，也為后續其他學者在用此方法進行更多自由度的機械手在進行搬運任務時的軌跡優化提供了研究思路。