基于自適應(yīng)radau偽譜法的機(jī)械手軌跡規(guī)劃方法研究

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

（河北工業(yè)大學(xué) 機(jī)械工程學(xué)院，天津 300132）

0 引言

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

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

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

1 動力學(xué)建模與Bolza型最優(yōu)控制描述

利用拉格朗日方程可得機(jī)械臂的動力學(xué)模型為：

q為機(jī)械臂的關(guān)節(jié)位移，q為機(jī)械臂的關(guān)節(jié)速度，u為關(guān)節(jié)力矩，將動力學(xué)方程轉(zhuǎn)化為狀態(tài)方程，令x為狀態(tài)向量，u為控制向量，x1=q，x2=q，x=[x1，x2]，建立狀態(tài)方程如下：

將動力學(xué)方程轉(zhuǎn)為狀態(tài)方程可得：

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

1.1 二自由度機(jī)械手動力學(xué)模型

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

圖1 二自由度機(jī)械手簡化模型

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

其中：

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

表1 二自由度機(jī)械手參數(shù)

1.2 三自由度機(jī)械手動力學(xué)模型

三自由度機(jī)械手經(jīng)簡化后建立如圖3所示坐標(biāo)系：

圖3 三自由度機(jī)械臂簡化模型

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

其中：

相對于二自由度機(jī)械手而言，主要是增加了旋轉(zhuǎn)臂的部分，旋轉(zhuǎn)臂部分的參數(shù)表如表2所示：

表2 旋轉(zhuǎn)臂部分參數(shù)

1.3 Bolza型最優(yōu)控制模型

Bolza型最優(yōu)控制問題[12]可定義如下：

約束條件為：

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

表3 機(jī)械手約束

本文以機(jī)械手關(guān)節(jié)力矩的平方和最小為優(yōu)化目標(biāo)即：

式(10)中ui(t)表示機(jī)械臂第i個(gè)關(guān)節(jié)在t時(shí)刻的關(guān)節(jié)力矩。n表示機(jī)械臂的總關(guān)節(jié)數(shù)。

2 自適應(yīng)radau偽譜法

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

2.1 radau偽譜法

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

N次Legendre多項(xiàng)式RN(τ)的表達(dá)式為：

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

拉格朗日基函數(shù)為：

然后應(yīng)用N階拉格朗日多項(xiàng)式對最優(yōu)控制問題的狀態(tài)變量和控制變量進(jìn)行類似：

其中x為狀態(tài)變量，X為近似狀態(tài)變量，u為控制變量，U為近似控制變量。

對式(14)求微分，可得狀態(tài)變量對時(shí)間的導(dǎo)數(shù)為：

其中：

其中：

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

由式(16)將狀態(tài)方程進(jìn)行離散可得：

本研究所用的機(jī)械手是以關(guān)節(jié)力矩平方和作為目標(biāo)函數(shù)，目標(biāo)函數(shù)經(jīng)過離散得：

2.2 自適應(yīng)原則

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

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

其中X為任意整數(shù)常量，控制每一段內(nèi)配點(diǎn)數(shù)的增長數(shù)，而ceil是舍入到下一個(gè)較大整數(shù)的運(yùn)算符。

若通過進(jìn)一步劃分段來修改配點(diǎn)的數(shù)量和分布則子區(qū)間段的個(gè)數(shù)可表示為：

其中Y為任意的整數(shù)常數(shù)，來控制子區(qū)間段的個(gè)數(shù)。而ceil是將參數(shù)舍入為+∞的運(yùn)算符。

3 系統(tǒng)仿真

3.1 仿真設(shè)定

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

3.2 仿真結(jié)果

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

圖4 機(jī)械手五次多項(xiàng)式軌跡規(guī)劃

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

圖5 二自由度機(jī)械手偽譜法與五次多項(xiàng)式關(guān)節(jié)力矩對比

圖6 三自由度機(jī)械手偽譜法與五次多項(xiàng)關(guān)節(jié)力矩對比

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

4 實(shí)驗(yàn)

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

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

圖7 實(shí)驗(yàn)場景

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

圖8 關(guān)節(jié)力矩對比實(shí)驗(yàn)

5 結(jié)語

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