基于POE建模的工業(yè)機(jī)器人最優(yōu)速度規(guī)劃

吳浩天，黃思，肖湘桂，楊建中

（華中科技大學(xué)國家數(shù)控系統(tǒng)工程技術(shù)研究中心，武漢 430074）

0 引言

工業(yè)機(jī)器人速度規(guī)劃[1]分為路徑未知和路徑已知兩種情況，前者主要用于點(diǎn)到點(diǎn)的運(yùn)動(dòng)，后者廣泛用于焊接、噴涂等工藝上。如何在確定路徑的情況下規(guī)劃機(jī)器人的速度使得時(shí)間盡可能短成為工業(yè)和學(xué)術(shù)界廣泛關(guān)注的問題。一類被廣泛應(yīng)用的方法是采用改進(jìn)遺傳算法[2]、改進(jìn)粒子群算法[3]等進(jìn)行問題求解，此類算法效率相對(duì)較低，很難直接應(yīng)用到工業(yè)生產(chǎn)；還有諸如罰函數(shù)法[4]、序列二次規(guī)劃[5]等方法，其求解精度過分依賴于初始條件，無法保證全局最優(yōu)。上述方法受制于算法本身存在的缺陷，很難同時(shí)保證機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性。常用來解決此類優(yōu)化問題的方法還有基于凸優(yōu)化[6]和可達(dá)性分析[7]的最優(yōu)速度規(guī)劃；除此之外隨著人工智能的發(fā)展，神經(jīng)網(wǎng)絡(luò)[8]、強(qiáng)化學(xué)習(xí)[9]等方法也被廣泛應(yīng)用，關(guān)鍵是如何將機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性充分發(fā)揮。

本文未采用傳統(tǒng)DH參數(shù)法[10]建立機(jī)器人運(yùn)動(dòng)學(xué)模型，而是采用指數(shù)乘積公式POE（The Product of Exponentials Formul）[11]，利用其具有一階連續(xù)的良好性質(zhì)，與李群李代數(shù)理論聯(lián)系起來，方便擴(kuò)展到動(dòng)力學(xué)建模。在得到機(jī)器人全局模型之后，以時(shí)間為主要優(yōu)化目標(biāo)，綜合考慮機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)約束，基于凸優(yōu)化理論，將目標(biāo)問題轉(zhuǎn)化為標(biāo)準(zhǔn)的二階錐規(guī)劃[12]模型并完成目標(biāo)問題的求解，最后仿真結(jié)果顯示，本文方法在時(shí)間上有很大優(yōu)勢(shì)，可為工業(yè)機(jī)器人的實(shí)際應(yīng)用提供理論依據(jù)。

1 機(jī)器人全局POE運(yùn)動(dòng)學(xué)分析

以國產(chǎn)華數(shù)機(jī)器人HSR-605為研究對(duì)象，采用指數(shù)積建模的方法建立機(jī)器人運(yùn)動(dòng)學(xué)模型，該方法基于指數(shù)積公式，將運(yùn)動(dòng)方程表示成運(yùn)動(dòng)旋量的指數(shù)積，由此給出了機(jī)器人運(yùn)動(dòng)學(xué)方程完整幾何表達(dá)。該方法對(duì)比傳統(tǒng)DH法，無需建立局部坐標(biāo)系，大大簡化了建模過程，且模型具有更明確的物理和幾何意義。下面結(jié)合指數(shù)積旋量理論針對(duì)機(jī)械臂正逆運(yùn)動(dòng)學(xué)進(jìn)行分析。

HSR-605的指數(shù)積建模及尺寸關(guān)系如圖1所示。

圖1 HSR-605機(jī)器人連桿坐標(biāo)系

機(jī)器人基座標(biāo)系{0}的坐標(biāo)軸方向?yàn)閄0、Y0、Z0，機(jī)器人末端法蘭盤坐標(biāo)系{6}的坐標(biāo)軸方向?yàn)閄6、Y6、Z6。L1、L2、L3、L4、L5、L6分別表示機(jī)器人指數(shù)積建模時(shí)各個(gè)關(guān)節(jié)軸線的線矢量，其方向如圖2所示，其中L4、L5、L6相交于同一點(diǎn)。a2=360 mm，a3=90 mm，d4=376.5 mm，d6=119 mm。六關(guān)節(jié)機(jī)器人的指數(shù)積運(yùn)動(dòng)學(xué)方程如下：

圖2 指數(shù)積建模及尺寸關(guān)系

式中：Li為運(yùn)動(dòng)旋量坐標(biāo)；Li［］為運(yùn)動(dòng)旋量；ωi為第i關(guān)節(jié)旋轉(zhuǎn)軸在基座標(biāo)系中的軸線方向矢量；ri為第i 關(guān)節(jié)軸線上的一點(diǎn)在基座標(biāo)系中的位置坐標(biāo)。

機(jī)器人運(yùn)動(dòng)學(xué)的正解映射關(guān)系如下：

2 時(shí)間最優(yōu)速度規(guī)劃

本小節(jié)首先進(jìn)行參數(shù)化處理，借助弦長參數(shù)化理論通過引入路徑參數(shù)s、s˙、s¨將6n維優(yōu)化問題轉(zhuǎn)化成相平面s˙-s、s¨-s上的2維問題，大大簡化后續(xù)的求解過程。根據(jù)鏈?zhǔn)角髮?dǎo)法則q˙=q′（s）s˙、q¨=q″（s）s˙2+q′（s）s¨，將關(guān)節(jié)空間問題轉(zhuǎn)換到參數(shù)空間進(jìn)行求解。

2.1 最優(yōu)速度規(guī)劃問題建模

以時(shí)間作為主要的優(yōu)化目標(biāo)以保證時(shí)間最優(yōu)，且在工業(yè)機(jī)器人作業(yè)的大多數(shù)環(huán)境當(dāng)中以犧牲時(shí)間效率為代價(jià)去節(jié)省能量意義不大，故本文并未引入能量項(xiàng)；而考慮到當(dāng)時(shí)間處于最優(yōu)狀態(tài)時(shí)機(jī)器人至少有一個(gè)關(guān)節(jié)軸會(huì)處于最大加速度或最大減速度狀態(tài)，容易產(chǎn)生沖擊、抖動(dòng)等，破壞機(jī)械結(jié)構(gòu)，這是由于機(jī)器人具有bang-bang特性[14]。故引入沖擊項(xiàng)來使規(guī)劃后的速度相對(duì)平緩，且乘上比例因子μ來控制權(quán)重。故參數(shù)化處理后時(shí)間最優(yōu)速度規(guī)劃問題的目標(biāo)函數(shù)為

基于第1節(jié)POE運(yùn)動(dòng)學(xué)分析的結(jié)論，將機(jī)器人關(guān)節(jié)約束和笛卡爾速度約束考慮在內(nèi)。其中關(guān)節(jié)約束主要考慮電動(dòng)機(jī)的性能上限，使每個(gè)關(guān)節(jié)軸均不會(huì)超速；由于工業(yè)機(jī)器人在焊接、噴涂等領(lǐng)域作業(yè)時(shí)更多關(guān)注的是末端TCP的工作狀態(tài)，故此處引入TCP笛卡爾速度約束。運(yùn)動(dòng)學(xué)約束為：

2.2 凸優(yōu)化問題求解

在得到時(shí)間最優(yōu)速度規(guī)劃問題的目標(biāo)函數(shù)和約束條件后，基于凸優(yōu)化理論進(jìn)行問題的分析和求解。如式（5）～式（7）所示，為實(shí)現(xiàn)將非線性目標(biāo)問題線性化，且引入仿射函數(shù)作為約束來進(jìn)行凸優(yōu)化分析，本文將參數(shù)速率b（s）=s˙2和參數(shù)加速率a（s）=s¨作為優(yōu)化目標(biāo)，根據(jù)不等式約束，將τi（s）用動(dòng)力學(xué)方程（7）代替后，可以簡單畫出此優(yōu)化問題在相平面上的可行域，如圖3所示。

圖3 不等式約束可行域分析

根據(jù)路徑參數(shù)方程對(duì)路徑參數(shù)s進(jìn)行等誤差離散成k份，在轉(zhuǎn)矩約束中參數(shù)均取為區(qū)間平均值，且根據(jù)參數(shù)速率和參數(shù)加速率之間的關(guān)系，動(dòng)力學(xué)方程經(jīng)過一系列變換后離散化目標(biāo)凸優(yōu)化問題如下：

上述優(yōu)化問題的優(yōu)化變量為離散后的參數(shù)速率b（s），考慮到目前已有如SEDUMI[15]、YILMIP[16]等非常成熟的標(biāo)準(zhǔn)二階錐規(guī)劃庫來求解此類優(yōu)化問題，故仿真部分基于凸優(yōu)化子問題，二階錐規(guī)劃來進(jìn)行求解。

3 仿真結(jié)果與分析

借助SEDUMI標(biāo)準(zhǔn)工具包進(jìn)行二階錐問題求解，最終得到的是各個(gè)離散點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角位置、關(guān)節(jié)角速度及關(guān)節(jié)角加速度。利用機(jī)器人離線編程軟件InteRobot生成圖4所示的路徑，將弦高誤差設(shè)為0.01 mm，根據(jù)等誤差離散后的結(jié)果，可得到312個(gè)路徑點(diǎn)。

圖4 離線生成路徑曲線

根據(jù)第一節(jié)所述機(jī)器人POE逆運(yùn)動(dòng)學(xué)計(jì)算各關(guān)節(jié)位置信息。機(jī)器人現(xiàn)有的插補(bǔ)算法下規(guī)劃結(jié)果和優(yōu)化后仿真結(jié)果如圖5～圖6所示。

圖5 機(jī)器人原有的規(guī)劃算法下關(guān)節(jié)角速度

圖6 優(yōu)化后關(guān)節(jié)角速度

根據(jù)優(yōu)化前后關(guān)節(jié)角速度對(duì)比曲線可知，從時(shí)間上來說，優(yōu)化后的關(guān)節(jié)角速度與原有的規(guī)劃算法下關(guān)節(jié)角速度相比運(yùn)行時(shí)間更短，且抖動(dòng)有明顯好轉(zhuǎn)。

下面對(duì)沖擊項(xiàng)的比例系數(shù)進(jìn)行合適的選?。ㄈ绫?），若單純考慮時(shí)間最優(yōu)運(yùn)行時(shí)間為3.5 s，但此時(shí)有一個(gè)關(guān)節(jié)軸會(huì)處于最大加速狀態(tài)，比較μ=0和μ=0.1兩個(gè)狀態(tài)下的參數(shù)加速率a（s）值如圖7所示，能看出合適選取沖擊項(xiàng)的比例系數(shù)能對(duì)加速度突變有明顯改善。實(shí)驗(yàn)結(jié)果表明，當(dāng)μ=0.1時(shí)機(jī)器人會(huì)處于一個(gè)關(guān)節(jié)抖動(dòng)相對(duì)理想的最大化效率運(yùn)行狀態(tài)。

表1 最優(yōu)速度規(guī)劃時(shí)間結(jié)果對(duì)比 s

圖7 μ=0、0.1下的參數(shù)加速率a（s）值

4 結(jié)論

通過指數(shù)積建模的方法建立機(jī)器人運(yùn)動(dòng)學(xué)模型，提升準(zhǔn)確性的同時(shí)保證了計(jì)算效率，且綜合考慮機(jī)器人的運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)約束，解決給定路徑下機(jī)器人的時(shí)間最優(yōu)速度規(guī)劃問題。仿真結(jié)果表明，與華數(shù)機(jī)器人現(xiàn)有的插補(bǔ)算法對(duì)比，該方法在縮短原有算法運(yùn)行時(shí)間的基礎(chǔ)上減少了關(guān)節(jié)抖動(dòng)。此方法可為工業(yè)機(jī)器人在噴涂、打磨等領(lǐng)域的應(yīng)用提供理論基礎(chǔ)。