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

王 帥，楊 冬，陳貴亮，唐潤(rùn)智

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

其中：

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

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

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

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

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

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

其中：

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

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

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

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

約束條件為：

式(9)中，t0表示起始時(shí)間，tf表示終止時(shí)間，約束條件中x表示狀態(tài)約束，C表示路徑約束，φ表示邊界約束。就本研究的機(jī)械手而言，狀態(tài)約束即為對(duì)機(jī)械手各關(guān)節(jié)的位置和速度的起始、終止?fàn)顟B(tài)約束，邊界約束即限制位置、速度、關(guān)節(jié)力矩的邊界條件，假設(shè)空間內(nèi)無(wú)障礙物，所以無(wú)路徑約束。機(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)化問(wèn)題，本文采用自適應(yīng)radau偽譜法進(jìn)行關(guān)節(jié)型機(jī)械手的軌跡優(yōu)化。偽譜法是將連續(xù)的最優(yōu)控制問(wèn)題離散化為非線性規(guī)劃問(wèn)題，以拉格朗日多項(xiàng)式在離散點(diǎn)之間逼近狀態(tài)變量和控制變量，求得控制變量和狀態(tài)變量最優(yōu)解[13]；在解決復(fù)雜問(wèn)題時(shí)，約束條件也在增加，最優(yōu)控制問(wèn)題的求解速度也在不斷下降，而自適應(yīng)則是在求解過(guò)程中動(dòng)態(tài)調(diào)整網(wǎng)格區(qū)間個(gè)數(shù)和多項(xiàng)式階數(shù)來(lái)實(shí)現(xiàn)快速收斂[14]。相比于其他方法，自適應(yīng)radau偽譜法不需要提前對(duì)軌跡進(jìn)行插值擬合，存在著對(duì)狀態(tài)變量和控制變量求解精度高、收斂速度快的特點(diǎn)。

2.1 radau偽譜法

自適應(yīng)radau偽譜法首先將最優(yōu)控制問(wèn)題的時(shí)間t∈[t0,tf]通過(guò)變換映射到偽譜法所要求的時(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)式對(duì)最優(yōu)控制問(wèn)題的狀態(tài)變量和控制變量進(jìn)行類似：

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

對(duì)式(14)求微分，可得狀態(tài)變量對(duì)時(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)過(guò)離散得：

2.2 自適應(yīng)原則

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

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

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

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

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

3 系統(tǒng)仿真

3.1 仿真設(shè)定

針對(duì)機(jī)械手的負(fù)載能力提升問(wèn)題，在軌跡規(guī)劃時(shí)以降低機(jī)械手的關(guān)節(jié)力矩為目標(biāo)，以二自由度機(jī)械手和三自由度機(jī)械手模型為例，以關(guān)節(jié)位置、速度作為狀態(tài)變量，關(guān)節(jié)力矩作為控制變量，采用自適應(yīng)radau偽譜法對(duì)機(jī)械手進(jìn)行軌跡優(yōu)化，首先設(shè)置機(jī)械手的狀態(tài)約束和邊界約束，具體參數(shù)如表3所示，然后利用已建立的機(jī)械手動(dòng)力學(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)行對(duì)比，驗(yàn)證優(yōu)化方法的有效性，仿真時(shí)長(zhǎng)設(shè)置為1s。

3.2 仿真結(jié)果

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)語(yǔ)

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