抑制抖振的醫(yī)療機(jī)械臂軌跡規(guī)劃方法與仿真

文/黃相錦

1　引言

在工業(yè)4.0迅猛發(fā)展的大趨勢(shì)下，機(jī)械臂越來(lái)越廣泛地被應(yīng)用在工廠、實(shí)驗(yàn)室、醫(yī)療等智能環(huán)境中。由于能夠執(zhí)行運(yùn)輸各種零件、試驗(yàn)原料、醫(yī)療物品等的任務(wù)，代替工人進(jìn)行體力勞動(dòng)，因此機(jī)械臂能大幅提升應(yīng)用領(lǐng)域的自動(dòng)化水平。其中，機(jī)械臂的軌跡規(guī)劃作為對(duì)機(jī)械臂進(jìn)行導(dǎo)航的關(guān)鍵技術(shù)，直接決定了機(jī)械臂完成運(yùn)輸任務(wù)的質(zhì)量。

在上述應(yīng)用中，將機(jī)械臂應(yīng)用于無(wú)損診斷與檢測(cè)、微損精確定位操作、外科手術(shù)規(guī)劃與模擬、新型手術(shù)醫(yī)療方法等中，不僅促進(jìn)了傳統(tǒng)醫(yī)學(xué)的進(jìn)步，也帶動(dòng)了新技術(shù)、新理論的發(fā)展。

2　機(jī)械臂軌跡規(guī)劃總述

機(jī)械臂需要對(duì)操作步驟進(jìn)行設(shè)定來(lái)完成任務(wù)目標(biāo)，而軌跡規(guī)劃與設(shè)定操作步驟有著緊密的聯(lián)系。由此，機(jī)械臂的正常運(yùn)轉(zhuǎn)依賴于軌跡規(guī)劃，對(duì)機(jī)械臂進(jìn)行合理的軌跡規(guī)劃會(huì)對(duì)機(jī)械臂的運(yùn)轉(zhuǎn)過(guò)程以及作業(yè)效率產(chǎn)生至關(guān)重要的影響。

機(jī)械臂的軌跡是指，在運(yùn)動(dòng)過(guò)程中，其末端的位移、速度以及加速度。軌跡規(guī)劃是指，基于運(yùn)動(dòng)學(xué)原理在笛卡爾空間中和關(guān)節(jié)坐標(biāo)空間中根據(jù)目標(biāo)要求計(jì)算出預(yù)期的運(yùn)動(dòng)軌跡。在機(jī)械臂的整體設(shè)計(jì)規(guī)劃中，軌跡規(guī)劃是底層規(guī)劃。

圖1：醫(yī)療機(jī)械臂關(guān)節(jié)示意圖

一般而言，在對(duì)兩個(gè)預(yù)定的端點(diǎn)之間的軌跡進(jìn)行規(guī)劃時(shí)，能夠得到多條不同的軌跡。例如，機(jī)械臂既能夠沿將起始位置和終點(diǎn)位置相連的直線進(jìn)行運(yùn)動(dòng)，也能夠在光滑的圓弧形軌跡上進(jìn)行運(yùn)動(dòng)。

既可以在關(guān)節(jié)坐標(biāo)空間中進(jìn)行軌跡規(guī)劃，也可以在笛卡爾空間中進(jìn)行軌跡規(guī)劃。但是，無(wú)論哪種軌跡規(guī)劃方式都需要得到連續(xù)且平滑的運(yùn)動(dòng)軌跡。

基于笛卡爾空間的軌跡規(guī)劃，是將執(zhí)行器的位置和姿態(tài)、速度、加速度等表示為時(shí)間的函數(shù)，而對(duì)應(yīng)的關(guān)節(jié)位置、速度及加速度等由執(zhí)行器的時(shí)間函數(shù)導(dǎo)出。笛卡爾空間的軌跡規(guī)劃具有直觀性強(qiáng)，規(guī)劃的路徑符合度較高，容易實(shí)現(xiàn)連續(xù)路徑控制的特點(diǎn)。但是，由于機(jī)械臂的運(yùn)動(dòng)驅(qū)動(dòng)需要在關(guān)節(jié)空間這一級(jí)中實(shí)現(xiàn)，因此笛卡爾空間的規(guī)劃需要通過(guò)逆運(yùn)動(dòng)學(xué)矩陣和逆雅可比矩陣將規(guī)劃的結(jié)果映射到關(guān)節(jié)空間，運(yùn)算量大；并且，在映射中還可能產(chǎn)生奇異點(diǎn)和反解的非一致性等問(wèn)題。此外，由于傳感器對(duì)各關(guān)節(jié)構(gòu)件在歐氏空間中的精確位姿進(jìn)行定位較為困難，因此對(duì)于實(shí)時(shí)控制機(jī)械臂而言是很大的挑戰(zhàn)。

基于關(guān)節(jié)空間的軌跡規(guī)劃，是將關(guān)節(jié)變量表示為時(shí)間的函數(shù)來(lái)描述操作臂預(yù)期的運(yùn)動(dòng)。規(guī)劃得到的軌跡能直接作為驅(qū)動(dòng)給定指令，無(wú)需在笛卡爾空間坐標(biāo)系中對(duì)兩個(gè)路徑點(diǎn)之間的路徑形狀進(jìn)行描述，具有計(jì)算量小、效率高、實(shí)時(shí)性好的優(yōu)點(diǎn)；另一方面，基于關(guān)節(jié)空間的軌跡規(guī)劃只受關(guān)節(jié)速度和加速度的限制，并且，關(guān)節(jié)空間與直角坐標(biāo)空間之間的映射關(guān)系并不連續(xù)，因此并不存在奇異性等問(wèn)題。

本文將在關(guān)節(jié)空間探討對(duì)醫(yī)療機(jī)械臂進(jìn)行軌跡規(guī)劃的方法，研究如何確保速度和加速的連續(xù)性，保證關(guān)節(jié)較小沖擊及機(jī)械臂平穩(wěn)運(yùn)行和精確定位。

3　醫(yī)療機(jī)械臂模型設(shè)計(jì)

在對(duì)用于醫(yī)療領(lǐng)域的機(jī)械臂進(jìn)行設(shè)計(jì)時(shí)，為使其到達(dá)一定空間范圍內(nèi)的任意位置，機(jī)械臂一般應(yīng)具有3個(gè)自由度，由P（移動(dòng)關(guān)節(jié)）與R（旋轉(zhuǎn)關(guān)節(jié)）組合而成。另一方面，為了模仿人手并實(shí)現(xiàn)醫(yī)療機(jī)械臂的末端能夠調(diào)整至任意的姿態(tài)，通常需要6個(gè)主動(dòng)自由度的醫(yī)療機(jī)械臂完成位置和姿態(tài)的確定。本文提出的醫(yī)療機(jī)械臂模型如圖1所示。

4　多項(xiàng)式插值算法

4.1　軌跡規(guī)劃指標(biāo)

在對(duì)機(jī)械臂進(jìn)行軌跡規(guī)劃時(shí)，有若干反映軌跡規(guī)劃曲線特性的參數(shù)，對(duì)于機(jī)械臂的軌跡規(guī)劃過(guò)程以及機(jī)械臂的運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)特性的有重要的影響，分析這些參數(shù)具有重要的意義。這些參數(shù)主要包括最大速度、最大加速度以及最大沖擊等。

為了實(shí)現(xiàn)平穩(wěn)的機(jī)械臂定位過(guò)程，在進(jìn)行運(yùn)動(dòng)規(guī)劃時(shí)，除了需要確保位置和速度的連續(xù)性，在此基礎(chǔ)上，還需要保證加速度不出現(xiàn)突變，以避免在末端產(chǎn)生振動(dòng)、即所謂的抖振現(xiàn)象。各個(gè)關(guān)節(jié)的約束如下：

表1：路徑點(diǎn)的參數(shù)

式中，t0表示運(yùn)動(dòng)的起始時(shí)間，tf表示運(yùn)動(dòng)的終止時(shí)間，θ0表示起始點(diǎn)的關(guān)節(jié)角，θf(wàn)表示終止點(diǎn)的關(guān)節(jié)角，表示起始點(diǎn)的關(guān)節(jié)角速度，表示終止點(diǎn)的關(guān)節(jié)角速度，表示起始點(diǎn)的關(guān)節(jié)角加速度，表示終止點(diǎn)的關(guān)節(jié)角加速度。

通常，需要對(duì)關(guān)節(jié)運(yùn)動(dòng)進(jìn)行規(guī)劃，以控制機(jī)械臂的關(guān)節(jié)空間運(yùn)動(dòng)量，從而確保平滑的關(guān)節(jié)運(yùn)動(dòng)軌跡和平穩(wěn)的運(yùn)動(dòng)。其主要內(nèi)容包括關(guān)節(jié)運(yùn)動(dòng)軌跡的選擇和關(guān)節(jié)運(yùn)動(dòng)位置的插值。如圖2所示，某關(guān)節(jié)在t0時(shí)刻的關(guān)節(jié)位置為θ0，在tf時(shí)刻位置為θf(wàn)。

關(guān)節(jié)的運(yùn)動(dòng)軌跡可以有很多條，如圖2示出的軌跡1～3，按軌跡1和軌跡2運(yùn)動(dòng)，機(jī)械臂運(yùn)動(dòng)過(guò)程中會(huì)有波動(dòng)，這是不希望的。按軌跡3運(yùn)行，則機(jī)械臂能夠平穩(wěn)的由初始位置到達(dá)目標(biāo)位置。因此，通常選擇類(lèi)似軌跡3的軌跡，經(jīng)過(guò)插值后控制機(jī)械臂的運(yùn)動(dòng)。

4.2　三次多項(xiàng)式插值

考慮某關(guān)節(jié)從t0時(shí)刻的關(guān)節(jié)位置θ0運(yùn)動(dòng)到tf時(shí)刻的關(guān)節(jié)位置θf(wàn)，假設(shè)這兩個(gè)時(shí)刻的速度均為零，可以得到機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)的邊界條件，如下式（4）。

令關(guān)節(jié)位置位置為式（5）所示的三次多項(xiàng)式，對(duì)其求一階導(dǎo)得到式（6）所示的關(guān)節(jié)速度多項(xiàng)式。

將邊界條件式（4）代入式（5）和式（6），求解出系數(shù)a0-a3，將系數(shù)代入式（5）和（6）中，得到如式（7）所示的三次多項(xiàng)式插值的關(guān)節(jié)位置表達(dá)式以及如式（8）所示的關(guān)節(jié)速度表達(dá)式。

圖2：關(guān)節(jié)的軌跡規(guī)劃

然而，三次多項(xiàng)式插值方法只適用于起始點(diǎn)和終止點(diǎn)的速度為零的運(yùn)動(dòng)情況。一般情況下，運(yùn)動(dòng)軌跡在中間多個(gè)點(diǎn)有位姿要求，規(guī)劃的軌跡需要過(guò)路徑點(diǎn)。如果在每個(gè)通過(guò)的路徑點(diǎn)都進(jìn)行停留，則三次多項(xiàng)式插值方法完全適用。

4.3　五次多項(xiàng)式插值

但是，對(duì)于醫(yī)療領(lǐng)域，通常要求機(jī)械臂在運(yùn)轉(zhuǎn)過(guò)程中不做任何停留以提高作業(yè)效率。另外，對(duì)于尤其是精密醫(yī)療外科手術(shù)這樣的應(yīng)用場(chǎng)景，要求避免機(jī)械臂因加速度突變引起的抖振現(xiàn)象，三次多項(xiàng)式插值算法并不能滿足要求。

對(duì)于本文涉及的醫(yī)療機(jī)械臂而言，考慮到機(jī)械臂運(yùn)行的環(huán)境中沒(méi)有物體的遮擋，對(duì)空間軌跡沒(méi)有太高的要求，另一方面，要在保證關(guān)節(jié)速度、加速度連續(xù)的同時(shí)避免關(guān)節(jié)較大的沖擊。因此對(duì)應(yīng)用于醫(yī)療領(lǐng)域的機(jī)械臂提出采用五次多項(xiàng)式插值算法。

通過(guò)采用高階多項(xiàng)式插值方法將機(jī)械臂關(guān)節(jié)空間的起始點(diǎn)與目標(biāo)點(diǎn)的加速度的參數(shù)考慮在內(nèi)。在該情況下，機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)的邊界條件如下：

滿足上述條件的多項(xiàng)式插值函數(shù)需要6個(gè)系數(shù)，因此選擇五次多項(xiàng)式函數(shù)作為各關(guān)節(jié)軌跡的插值函數(shù)，其形式如式（10）所示。

對(duì)于五次多項(xiàng)式插值，不僅能實(shí)現(xiàn)運(yùn)動(dòng)過(guò)程的沖擊最小化，而且相應(yīng)的速度函數(shù)為相對(duì)于時(shí)間變量的四次函數(shù)，加速度函數(shù)為相對(duì)于時(shí)間變量的三次函數(shù)，能夠得到連續(xù)的速度曲線和加速度曲線，并且上述曲線具有較好的平滑性。

圖3：三次和五次插值的關(guān)節(jié)位置對(duì)比

圖4：三次和五次插值的關(guān)節(jié)速度對(duì)比

圖5：三次和五次插值的關(guān)節(jié)加速度對(duì)比

其中，在運(yùn)動(dòng)過(guò)程中，關(guān)節(jié)速度和加速度為：

最終得到：

5　插值算法的仿真

針對(duì)機(jī)械臂中的某一旋轉(zhuǎn)關(guān)節(jié)，使該關(guān)節(jié)依次通過(guò)如表1所示的三個(gè)點(diǎn)，其中，采樣周期為0.05s。在matlab中對(duì)過(guò)路徑點(diǎn)的三次多項(xiàng)式插值算法和五次多項(xiàng)式插值算法分別進(jìn)行仿真。如圖3～圖5示出仿真得到的關(guān)節(jié)位置、速度和加速度。

從仿真結(jié)果可以看出，三次多項(xiàng)式插值算法能夠保證各個(gè)點(diǎn)的關(guān)節(jié)角、角速度的連續(xù)性，但角加速度出現(xiàn)了突變。另一方面，五次多項(xiàng)式插值算法確保了加速度的連續(xù)性。因此在關(guān)節(jié)空間規(guī)劃算法中，通過(guò)采用五次多項(xiàng)式插值，并在規(guī)劃中合理地選取中間點(diǎn)，就能夠限制最大加速度、速度等，從而實(shí)現(xiàn)對(duì)運(yùn)動(dòng)平穩(wěn)性的控制。

6　結(jié)語(yǔ)

本文探討了實(shí)現(xiàn)加速度平滑過(guò)渡的醫(yī)療機(jī)械臂軌跡規(guī)劃方法。通過(guò)在關(guān)節(jié)空間對(duì)醫(yī)療機(jī)械臂采用五次多項(xiàng)式插值算法進(jìn)行軌跡規(guī)劃，確保速度和加速的連續(xù)性，保證關(guān)節(jié)較小沖擊及醫(yī)療機(jī)械臂平穩(wěn)運(yùn)行和精確定位。并且，在關(guān)節(jié)空間內(nèi)對(duì)過(guò)路徑點(diǎn)的三次多項(xiàng)式插值算法和五次多項(xiàng)式插值算法進(jìn)行了仿真分析，結(jié)果表明，五次多項(xiàng)式插值算法不僅能確保速度、加速度的連續(xù)，同時(shí)也保證關(guān)節(jié)沖擊在較小的范圍內(nèi)。證明五次多項(xiàng)式插值能夠滿足尤其是精密醫(yī)療外科手術(shù)這樣的應(yīng)用場(chǎng)景中的要求，避免醫(yī)療機(jī)械臂因加速度突變引起的抖振現(xiàn)象。