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

劉俊輝 ，周偉

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

0 前言

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

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

1 液壓機械臂空間運動求解與軌跡誤差分析

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

圖1 改進D-H算法下的機械臂關節(jié)坐標空間標定

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

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

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

(1)

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

(2)

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

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

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

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

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

(3)

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

=-+1

(4)

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

(5)

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

(6)

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

(7)

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

2.2 液壓機械臂空間軌跡過渡與插值

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

(8)

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

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

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

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

(cos-cos)

(9)

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

3 實驗結果與分析

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

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

圖3 六自由度液壓機械臂軌跡控制實驗現(xiàn)場

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

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

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

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

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

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

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

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

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

表3 六自由度機械臂各液壓關節(jié)的型值點標準數(shù)據(jù)

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

表4 三次B樣條算法下關節(jié)運動指標偏差

表5 力矩屬性建模算法下關節(jié)運動指標偏差

表6 模糊自適應算法下關節(jié)運動指標偏差

表7 模糊免疫PID算法下關節(jié)運動指標偏差

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

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

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

4 結束語

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