基于三次B樣條的六自由度液壓機(jī)械臂軌跡規(guī)劃

劉俊輝 ，周偉

(1.平頂山技師學(xué)院現(xiàn)代制造系，河南平頂山 467000； 2.鄭州機(jī)械研究所有限公司，河南鄭州 450001)

0 前言

當(dāng)前，在自動(dòng)化控制技術(shù)和計(jì)算機(jī)軟硬件技術(shù)的推動(dòng)下，工業(yè)機(jī)械臂的應(yīng)用場(chǎng)景越來(lái)越多。工業(yè)機(jī)械臂在產(chǎn)品加工精度、產(chǎn)品生產(chǎn)的一致性、高強(qiáng)度作業(yè)時(shí)長(zhǎng)、持續(xù)工作時(shí)長(zhǎng)及作業(yè)安全性等方面，與傳統(tǒng)的人工作業(yè)、半自動(dòng)加工方式相比優(yōu)勢(shì)十分明顯，顯著提升了產(chǎn)品的加工精度和生產(chǎn)效率。六自由度機(jī)械臂經(jīng)常被應(yīng)用于高精度的加工場(chǎng)景，但由于大多數(shù)機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)大都采用電機(jī)和減速器相結(jié)合的驅(qū)動(dòng)方式，機(jī)械臂的負(fù)重比較小，無(wú)法適用于大力矩的輸出場(chǎng)景，因此限制了機(jī)械臂應(yīng)用范圍的進(jìn)一步拓展。與電機(jī)驅(qū)動(dòng)關(guān)節(jié)相比，液壓系統(tǒng)具有更大的負(fù)載能力和更穩(wěn)定的傳動(dòng)平穩(wěn)性，可以有效提高機(jī)械臂的負(fù)重比，并增加六自由度機(jī)械臂的應(yīng)用場(chǎng)景。但六軸液壓機(jī)械臂在使用中也存在一定弊端，包括液壓系統(tǒng)的響應(yīng)遲滯及由于液壓關(guān)節(jié)系統(tǒng)給末端執(zhí)行器帶來(lái)的軌跡偏差。

關(guān)于液壓關(guān)節(jié)機(jī)械臂軌跡偏差的修正問(wèn)題，國(guó)內(nèi)外專(zhuān)家和學(xué)者進(jìn)行了廣泛和深入的研究。文獻(xiàn)[8]提出一種基于機(jī)械臂力矩計(jì)算的數(shù)學(xué)建模方法，通過(guò)控制運(yùn)動(dòng)中的機(jī)械臂各物理參數(shù)，動(dòng)態(tài)修正末端執(zhí)行器的軌跡偏差。但機(jī)械臂關(guān)節(jié)在高負(fù)載的場(chǎng)景會(huì)存在嚴(yán)重的擾動(dòng)現(xiàn)象，各核心參數(shù)的提取難度較大，影響模型的精確性。文獻(xiàn)[9]提出一種基于模糊自適應(yīng)控制的軌跡修正方案，但該算法要求將實(shí)時(shí)的機(jī)械臂移動(dòng)數(shù)據(jù)和軌跡修正數(shù)據(jù)傳遞給后臺(tái)控制系統(tǒng)，計(jì)算復(fù)雜度和控制成本過(guò)高，無(wú)法推廣到工業(yè)領(lǐng)域。文獻(xiàn)[10]提出以仿生算法為基礎(chǔ)的模糊免疫PID控制算法，并結(jié)合線性插值優(yōu)化機(jī)械臂的運(yùn)動(dòng)軌跡，但該控制方案的控制精度穩(wěn)定性低。分析現(xiàn)有控制算法的不足，并綜合考慮控制成本、算法復(fù)雜度、控制精度及推廣價(jià)值等因素，本文作者提出基于三次B樣條的六自由度機(jī)械臂軌跡修正方案。三次B樣條曲線修正的優(yōu)勢(shì)在于，它兼具了凸包性、三階可微及控制參數(shù)表達(dá)相對(duì)簡(jiǎn)潔等優(yōu)勢(shì)，可實(shí)現(xiàn)對(duì)機(jī)械臂末端執(zhí)行器速度、加速度和位置移動(dòng)軌跡的周期性插補(bǔ)，有效控制軌跡偏差。

1 液壓機(jī)械臂空間運(yùn)動(dòng)求解與軌跡誤差分析

六自由度機(jī)械臂的控制系統(tǒng)通過(guò)輸入液壓擺缸關(guān)節(jié)的指令，控制機(jī)械臂完成復(fù)雜的期望動(dòng)作。機(jī)械臂末端執(zhí)行器的運(yùn)動(dòng)軌跡，受各液壓關(guān)節(jié)空間坐標(biāo)和位姿的影響。利用改進(jìn)的D-H參數(shù)法，建立機(jī)械臂各液壓關(guān)節(jié)的坐標(biāo)系，并對(duì)各關(guān)節(jié)角作正向和逆向求解。對(duì)經(jīng)典的D-H算法進(jìn)行優(yōu)化，針對(duì)每個(gè)液壓關(guān)節(jié)單獨(dú)構(gòu)建坐標(biāo)系，且保持所構(gòu)建的坐標(biāo)系與六自由度機(jī)械臂關(guān)節(jié)一一對(duì)應(yīng)。經(jīng)過(guò)優(yōu)化和改進(jìn)的笛卡爾空間內(nèi)六軸機(jī)械臂的坐標(biāo)設(shè)定如圖1所示。

圖1 改進(jìn)D-H算法下的機(jī)械臂關(guān)節(jié)坐標(biāo)空間標(biāo)定

其中:和+1分別為液壓機(jī)械臂關(guān)節(jié)和關(guān)節(jié)+1所對(duì)應(yīng)的坐標(biāo)系原點(diǎn)；+1為關(guān)節(jié)+1圍繞軸的夾角；為液壓機(jī)械臂關(guān)節(jié)對(duì)應(yīng)連桿的扭角；為關(guān)節(jié)對(duì)應(yīng)的連桿長(zhǎng)度；+1為連桿+1所對(duì)應(yīng)的偏置。如果給定某一關(guān)節(jié)對(duì)應(yīng)的連桿長(zhǎng)度，可對(duì)應(yīng)求出其他機(jī)械臂液壓關(guān)節(jié)對(duì)應(yīng)的D-H參數(shù)。如果設(shè)為350 mm，則對(duì)應(yīng)的其他關(guān)節(jié)參數(shù)如表1所示。

表1 液壓機(jī)械臂參數(shù)(a3=350 mm)

依據(jù)表1可依次計(jì)算出相對(duì)于機(jī)械臂基座的各液壓關(guān)節(jié)空間位姿，進(jìn)而得到各關(guān)節(jié)空間位姿的變換矩陣。關(guān)節(jié)變換矩陣能夠描述出機(jī)械臂的空間位姿的正向運(yùn)動(dòng)情況，用表示關(guān)節(jié)1的空間位姿矩陣，則有：

(1)

同理可分別求解出六自由度機(jī)械臂液壓關(guān)節(jié)在笛卡爾空間內(nèi)的位姿矩陣～。機(jī)械臂末端執(zhí)行器的空間位姿由6個(gè)液壓關(guān)節(jié)空間位姿決定，將6個(gè)關(guān)節(jié)空間位姿矩陣相乘即可得到末端執(zhí)行器的空間位姿和運(yùn)動(dòng)軌跡。液壓機(jī)械臂存在關(guān)節(jié)柔性，機(jī)械臂關(guān)節(jié)繞軸旋轉(zhuǎn)會(huì)導(dǎo)致關(guān)節(jié)角增量的產(chǎn)生。每個(gè)關(guān)節(jié)角增量對(duì)應(yīng)一個(gè)偏移誤差矩陣，具體表示為

(2)

其中:為第個(gè)液壓關(guān)節(jié)對(duì)應(yīng)的旋轉(zhuǎn)誤差對(duì)應(yīng)的偏離角。通過(guò)掌握六軸機(jī)械臂液壓控制系統(tǒng)的周期性運(yùn)動(dòng)規(guī)律，能夠有效控制轉(zhuǎn)角誤差，并抑制軌跡偏差。液壓機(jī)械臂在關(guān)節(jié)結(jié)構(gòu)設(shè)計(jì)上與傳統(tǒng)電機(jī)驅(qū)動(dòng)關(guān)節(jié)不同，其液壓缸輸出軸與連桿直接連接，液壓缸的響應(yīng)滯后問(wèn)題會(huì)放大機(jī)械臂關(guān)節(jié)的轉(zhuǎn)角誤差。在固定的關(guān)節(jié)轉(zhuǎn)角運(yùn)動(dòng)度數(shù)前提下，需要通過(guò)實(shí)時(shí)調(diào)節(jié)和不斷的軌跡修正保持末端執(zhí)行器的控制精度。

2 基于三次B樣條的液壓機(jī)械臂軌跡修正

六自由度機(jī)械臂能夠完成更復(fù)雜和更高精度的指令動(dòng)作，但現(xiàn)有機(jī)械臂在行進(jìn)軌跡插值模式下，很難提高對(duì)軌跡局部的控制穩(wěn)定性。B樣條函數(shù)的理論基礎(chǔ)源于貝塞爾曲線，在保留貝塞爾曲線的幾何不變性等優(yōu)點(diǎn)的同時(shí)，B樣條函數(shù)還能夠有效地對(duì)機(jī)械臂軌跡作局部規(guī)劃與優(yōu)化。

2.1 三次B樣條函數(shù)的構(gòu)造

在六自由度液壓機(jī)械臂運(yùn)動(dòng)過(guò)程中，如果某個(gè)液壓關(guān)節(jié)位置序列點(diǎn)的空間坐標(biāo)發(fā)生偏差，僅會(huì)影響到運(yùn)動(dòng)軌跡的一部分，若對(duì)偏差及時(shí)修正則并不會(huì)影響到整個(gè)機(jī)械臂的運(yùn)動(dòng)軌跡控制精度。次B樣條函數(shù)()可以表示為

(3)

式中：,()為控制頂點(diǎn)對(duì)應(yīng)的次B樣條基函數(shù)，B樣條基函數(shù)可由德布爾遞推公式得出;為個(gè)型值點(diǎn)中第個(gè)控制頂點(diǎn)；為B樣條函數(shù)的節(jié)點(diǎn)向量。當(dāng)?shù)螖?shù)為時(shí)，控制頂點(diǎn)的數(shù)量滿足如下關(guān)系式：

=-+1

(4)

式中：為機(jī)械臂液壓關(guān)節(jié)的數(shù)量，在區(qū)間[,++1]內(nèi)，控制頂點(diǎn)位置的變化并不會(huì)導(dǎo)致區(qū)域內(nèi)機(jī)械臂軌跡出現(xiàn)偏差。B樣條函數(shù)將液壓機(jī)械臂的移動(dòng)軌跡分割為多段曲線，基函數(shù),()在分段曲線上滿足連續(xù)可微的要求，B樣條將折線控制在凸包范圍內(nèi)，()函數(shù)上的控制點(diǎn)-、-+1、…、都包括在凸包內(nèi)，控制頂點(diǎn)可以表示為

(5)

當(dāng)取值為3時(shí)，三次B樣條函數(shù)()可以表示為

(6)

按照D-H參數(shù)表(表1)相關(guān)數(shù)據(jù)，可以計(jì)算出當(dāng)前狀態(tài)下液壓機(jī)械臂各關(guān)節(jié)的空間位姿和末端執(zhí)行器的空間位置坐標(biāo)，在笛卡爾空間范圍內(nèi)同時(shí)能夠監(jiān)測(cè)到出現(xiàn)軌跡偏差的區(qū)域，并描繪出相關(guān)的示教點(diǎn)，生成對(duì)應(yīng)的三次B樣條曲線。將規(guī)劃出的三次B樣條曲線作為笛卡爾空間內(nèi)的機(jī)械臂目標(biāo)軌跡，目的是利用曲線的平滑功能控制機(jī)械臂各關(guān)節(jié)、末端執(zhí)行器的位置偏差，并控制液壓機(jī)械臂的移動(dòng)速度、加速度。所構(gòu)造的三次B樣條節(jié)點(diǎn)向量滿足公式(4)的數(shù)量要求，由于取值為3，所以三次B樣條中間頂點(diǎn)的間距為1(-2)，則B樣條函數(shù)的節(jié)點(diǎn)向量表示為

(7)

根據(jù)B樣條曲線中當(dāng)前節(jié)點(diǎn)向量的分布情況，可確定出相關(guān)的函數(shù)參數(shù)。針對(duì)三次B樣條曲線擬合的特征而言，每相鄰的曲線共用控制點(diǎn)，以控制點(diǎn)為中心構(gòu)造首末端點(diǎn)。具體步驟如下：要求出相鄰示教點(diǎn)的距離，并累計(jì)求弦長(zhǎng)之和；根據(jù)弦長(zhǎng)的比例進(jìn)一步分析出示教點(diǎn)的分布情況；依據(jù)公式(7)求出B樣條函數(shù)節(jié)點(diǎn)向量的表達(dá)式。

2.2 液壓機(jī)械臂空間軌跡過(guò)渡與插值

三次B樣條曲線函數(shù)是關(guān)于向量的表達(dá)式，應(yīng)根據(jù)機(jī)械臂的規(guī)劃速度和實(shí)際位移，得到向量隨時(shí)間變化的數(shù)值序列，進(jìn)而獲取三次B樣條曲線函數(shù)的空間位置點(diǎn)。由B樣條曲線函數(shù)的特性可知，當(dāng)節(jié)點(diǎn)向量為0或1時(shí)，其一階導(dǎo)數(shù)為0且在該B樣條曲線上一定存在極值點(diǎn)。液壓機(jī)械臂的末端執(zhí)行器在通過(guò)B樣條曲線極值點(diǎn)時(shí)需要控制速度，并按照修正后的軌跡行進(jìn)，末端執(zhí)行器在曲率極值點(diǎn)的最大通過(guò)速度為

(8)

式中：為三次B樣條曲線的曲率半徑；為機(jī)械臂運(yùn)動(dòng)中存在的最大法向量。以曲率極值點(diǎn)為中心對(duì)B樣條曲線作分段規(guī)劃，如果曲線中包含多個(gè)速度控制點(diǎn)，以該段規(guī)劃的末速度作為下一段的初速度，機(jī)械臂軌跡運(yùn)動(dòng)中對(duì)速度、加速度的控制是避免軌跡偏差過(guò)大及后續(xù)插值的基礎(chǔ)。根據(jù)控制系統(tǒng)的最大法向量加速度識(shí)別出三次B樣條函數(shù)曲線上的最優(yōu)限速點(diǎn)，并對(duì)曲線作分段處理。三次B樣條曲線的空間軌跡過(guò)渡過(guò)程描述如圖2所示。

圖2 三次B樣條曲線空間過(guò)渡過(guò)程描述

在圖2中，、、和4個(gè)控制點(diǎn)能夠確定一條三次B樣條曲線，機(jī)械臂的運(yùn)動(dòng)軌跡包含直線、過(guò)渡區(qū)和直線3個(gè)部分。利用曲線的曲率極值確定液壓機(jī)械臂軌跡的限速點(diǎn)并進(jìn)行分段規(guī)劃，以曲線為過(guò)渡區(qū)，確定機(jī)械臂末端執(zhí)行器在該曲線段初始速度和末速度。曲線段內(nèi)理論上存在+1個(gè)節(jié)點(diǎn)，即在該區(qū)間內(nèi)存在一個(gè)二階導(dǎo)函數(shù)()，可以構(gòu)成三角函數(shù)多項(xiàng)式，則函數(shù)()是三次B樣條函數(shù)()的插值函數(shù)。在給定的[0,π/2]區(qū)間內(nèi)，4個(gè)控制點(diǎn)對(duì)應(yīng)的插值函數(shù)一階導(dǎo)數(shù)值分布為、、和，則用三角函數(shù)來(lái)描述插值函數(shù)為

()=cos+sin+(sin-sin)+

(cos-cos)

(9)

以峰值點(diǎn)為中心,將三次B樣條曲線分割成兩個(gè)部分，且保證型值點(diǎn)在所分割的兩段曲線內(nèi)可導(dǎo)，根據(jù)端點(diǎn)的斜率可求解出控制點(diǎn)的斜率。插值后的B樣條曲線光滑度提升，且能夠保證B樣條插值函數(shù)在二階、三階連續(xù)。在笛卡爾空間內(nèi)，六自由度液壓機(jī)械臂接收控制系統(tǒng)指令后會(huì)出現(xiàn)一定延遲和局部軌跡偏差，針對(duì)軌跡偏差的特點(diǎn)，通過(guò)構(gòu)造三次B樣條函數(shù)修正機(jī)械臂末端執(zhí)行器的軌跡偏差，并在局部軌跡范圍內(nèi)保證關(guān)節(jié)運(yùn)動(dòng)參數(shù)調(diào)整的連貫性。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 實(shí)驗(yàn)環(huán)境與參數(shù)設(shè)定

在實(shí)驗(yàn)室的仿真環(huán)境下，驗(yàn)證三次B樣條算法對(duì)于機(jī)械臂軌跡控制的效果。六自由度機(jī)械臂選用URur液壓協(xié)作式機(jī)械臂，具有較高的負(fù)重比和靈活性，實(shí)驗(yàn)現(xiàn)場(chǎng)如圖3所示。

圖3 六自由度液壓機(jī)械臂軌跡控制實(shí)驗(yàn)現(xiàn)場(chǎng)

實(shí)驗(yàn)中機(jī)械臂的軌跡路徑和標(biāo)準(zhǔn)控制數(shù)據(jù)集采用自建的方式，其中液壓機(jī)械臂關(guān)節(jié)和連桿控制系統(tǒng)的軟硬件配置如表2所示。

表2 液壓機(jī)械臂控制系統(tǒng)軟硬件參數(shù)

實(shí)驗(yàn)中隨機(jī)選定50個(gè)液壓機(jī)械臂目標(biāo)位置,分為5組，其中4組數(shù)據(jù)作為訓(xùn)練集，1組數(shù)據(jù)作為測(cè)試集，測(cè)試中的最大迭代次數(shù)設(shè)定為700，最低訓(xùn)練誤差調(diào)整至10×10m。

3.2 機(jī)械臂軌跡規(guī)劃結(jié)果分析

液壓機(jī)械臂理論上的規(guī)劃軌跡會(huì)全部通過(guò)選定的50個(gè)目標(biāo)位置點(diǎn)，但在機(jī)械臂實(shí)際運(yùn)行中由于液壓關(guān)節(jié)的遲滯,位置軌跡會(huì)發(fā)生偏差。為驗(yàn)證所提算法的性能，利用機(jī)器人工具箱構(gòu)建完整的機(jī)械臂對(duì)象，并與傳統(tǒng)算法進(jìn)行對(duì)比。在笛卡爾空間范圍內(nèi)構(gòu)建6個(gè)液壓關(guān)節(jié)的初始位姿坐標(biāo)系；借助MATLAB仿真系統(tǒng)構(gòu)建D-H參數(shù)矩陣，并實(shí)時(shí)地顯示出機(jī)械臂的位姿和軌跡在、和面的偏離情況，如圖4—圖6所示。

由圖4—圖6可知：所提出的三次B樣條算法能夠有效實(shí)現(xiàn)3個(gè)軸向的軌跡糾偏，軌跡規(guī)劃處理后所篩選到的軌跡坐標(biāo)點(diǎn)，都能夠相對(duì)準(zhǔn)確地落到理論規(guī)劃的軌跡路徑上；而經(jīng)過(guò)3種傳統(tǒng)軌跡規(guī)劃與糾偏算法處理后，有一部分機(jī)械臂的軌跡坐標(biāo)點(diǎn)與理論規(guī)劃曲線之間存在一定程度的偏差，還存在較大的優(yōu)化空間。

圖4 xz平面各算法的軌跡糾偏情況 圖5 yz平面各算法的軌跡糾偏情況

圖6 xy平面各算法的軌跡糾偏情況

根據(jù)各液壓關(guān)節(jié)在最優(yōu)行進(jìn)路徑點(diǎn)上的角度，確定最佳的時(shí)間間隔，并實(shí)時(shí)控制和糾正機(jī)械臂的軌跡偏差。各液壓關(guān)節(jié)之間在型值點(diǎn)的標(biāo)準(zhǔn)數(shù)據(jù)，如表3所示。

表3 六自由度機(jī)械臂各液壓關(guān)節(jié)的型值點(diǎn)標(biāo)準(zhǔn)數(shù)據(jù)

以機(jī)械臂液壓關(guān)節(jié)型值點(diǎn)的標(biāo)準(zhǔn)數(shù)據(jù)為基準(zhǔn)，分析在不同的運(yùn)動(dòng)軌跡修正算法下，10個(gè)測(cè)試目標(biāo)位置點(diǎn)機(jī)械臂各關(guān)節(jié)最大力矩、最大角速度及最大加速度的偏差情況，結(jié)果如表4—表7所示。

表4 三次B樣條算法下關(guān)節(jié)運(yùn)動(dòng)指標(biāo)偏差

表5 力矩屬性建模算法下關(guān)節(jié)運(yùn)動(dòng)指標(biāo)偏差

表6 模糊自適應(yīng)算法下關(guān)節(jié)運(yùn)動(dòng)指標(biāo)偏差

表7 模糊免疫PID算法下關(guān)節(jié)運(yùn)動(dòng)指標(biāo)偏差

在機(jī)械臂目標(biāo)測(cè)試點(diǎn)的軌跡糾偏中，經(jīng)過(guò)三次B樣條算法修正后各關(guān)節(jié)的力矩偏差、角速度偏差和加速度偏差都被控制在較低范圍內(nèi)。以液壓機(jī)械臂的關(guān)節(jié)3為例，力矩偏差保持在9.7 N·mm，角速度偏差和加速度偏差分別為0.13(°)/s和0.90(°)/s，與其他3種軌跡糾偏方法相比具有較為明顯的優(yōu)勢(shì)。最后，驗(yàn)證軌跡糾偏中各控制算法的收斂性，基于MATLAB軟件得到的仿真結(jié)果如圖7所示。

圖7 不同算法軌跡糾偏的收斂性能對(duì)比

三次B樣條軌跡算法在400次迭代之內(nèi)就完成了收斂，具有更高的軌跡調(diào)整效率。其他3種軌跡修正方法的迭代次數(shù)均超過(guò)了550次，其中模糊自適應(yīng)算法甚至在最高迭代次數(shù)范圍沒(méi)有完成收斂。結(jié)果表明：三次B樣條算法在液壓機(jī)械臂軌跡規(guī)劃中兼具了行進(jìn)軌跡糾偏的準(zhǔn)確性和迭代效率，能夠有效解決液壓機(jī)械臂的響應(yīng)遲滯問(wèn)題。

4 結(jié)束語(yǔ)

配備液壓關(guān)節(jié)的六自由度機(jī)械臂具有更高的負(fù)重比，能夠使工業(yè)機(jī)械臂的應(yīng)用領(lǐng)域得到拓展，但液壓關(guān)節(jié)存在明顯的響應(yīng)時(shí)滯問(wèn)題，需要實(shí)時(shí)糾正機(jī)械臂軌跡出現(xiàn)的偏差問(wèn)題。本文作者通過(guò)構(gòu)建三次B樣條函數(shù)，對(duì)機(jī)械臂空間軌跡進(jìn)行插值和糾偏處理，可改善液壓機(jī)械臂軌跡出現(xiàn)的局部偏差問(wèn)題。實(shí)驗(yàn)結(jié)果也從液壓關(guān)節(jié)力矩偏差、加速度偏差等角度，驗(yàn)證了文中提出的軌跡糾偏算法的有效性和適用性。