本發(fā)明涉及冗余機械臂的重復(fù)運動規(guī)劃及控制技術(shù)，具體地，涉及一種有限時間收斂性能指標(biāo)、在初始偏移情形下的冗余機械臂的逆運動學(xué)求解方法。

背景技術(shù)：

機械臂是擬人手臂、手腕和手功能的機械電子裝置，其末端任務(wù)包括搬運、焊接、油漆和組裝等，目前已廣泛應(yīng)用于工業(yè)制造、醫(yī)學(xué)治療、娛樂服務(wù)、消防、軍事和太空探索等領(lǐng)域中。一個機械手臂一般擁有3個或3個以上的自由度，在用機械手臂去完成某個特定的工作而又多余的自由度時，該機械手臂被定義為冗余機械臂。同非冗余機械臂相比，冗余機械臂有更大的操作空間，多余的自由度能夠滿足更多的功能約束，比如物理極限躲避和環(huán)境障礙物躲避。

控制冗余機械臂實時運動的一個基本問題是冗余度解析方案。給出末端運動的軌跡，如何實時地得到各關(guān)節(jié)的速度、角速度和力矩值，經(jīng)典的做法是基于偽逆的冗余度解析方案?？紤]在m維空間中作業(yè)的具有n個自由度的機械臂，末端軌跡與關(guān)節(jié)位移之間的關(guān)系(即正運動學(xué)問題)

r(t)＝f(θ(t))

其中，r(t)表示機械臂末端執(zhí)行器在工作空間中笛卡爾坐標(biāo)系下的位移，θ(t)表示關(guān)節(jié)位移。末端笛卡爾空間與關(guān)節(jié)空間之間的微分運動關(guān)系為

其中，是r的時間導(dǎo)數(shù)，是關(guān)節(jié)速度向量，是機械臂的雅克比矩陣。

對于冗余機械臂，傳統(tǒng)方法是求解moore-penrose廣義逆(偽逆)，可得關(guān)節(jié)變量速度的最小二乘解為

這里，j⁺＝j(luò)^t(jj^t)^-1是雅克比矩陣j的偽逆。

d.e.whitney(resolvedmotionratecontrolofmanipulatorsandhumanprostheses,ieeetrans.man-machinesyst.,1969,10(2):47-53；(即：操縱器和人工假肢的運動速率控制方法)于1969年提出如下具有等式約束的最小速度范數(shù)性能指標(biāo)作為運動規(guī)劃的目標(biāo)函數(shù)：

式中，a為正定加權(quán)矩陣。求解上述規(guī)劃問題，需求解以下方程組

其解為

式(1)是式(3)當(dāng)a＝i時的特殊情形。也可看出，規(guī)劃問題是通過求解方程組(2)得到解決的。

冗余度解析的研究重點是冗余機械臂的軌跡規(guī)劃，其性能直接關(guān)系到機械臂能否成功完成給定的末端任務(wù)。當(dāng)末端執(zhí)行器的運動軌跡是閉合的，在機械臂完成末端工作任務(wù)后，各個關(guān)節(jié)角變量在運動空間中的軌跡不一定封閉。這種非重復(fù)性問題可能產(chǎn)生不期望的關(guān)節(jié)位形，使得冗余機械臂末端封閉軌跡的重復(fù)作業(yè)出現(xiàn)預(yù)料之外的情況，甚至?xí)?dǎo)致意外及危險情況的發(fā)生。應(yīng)用最為廣泛的偽逆控制法不能獲得運動的重復(fù)性。為了完成原有的重復(fù)運動，通常采用自運動的方法進行彌補,而自運動進行調(diào)整往往效率不高。(詳見kleincaandhuangc,reviewofpseudoinversecontrolforusewithkinematicallyredundantmanipulators.ieeetrans.syst.man.cybern.1983,13(2):245-250；即：基于偽逆控制方法的冗余機械臂運動規(guī)劃)。

基于二次優(yōu)化(quadraticoptimization,qp)的冗余解析方案受到關(guān)注，f.t.cheng于1994年提出關(guān)節(jié)無偏差性能指標(biāo)(f.-t.cheng,t.-h.chen,andy.-y.sun,resolvingmanipulatorredundancyunderinequalityconstraints,ieeetrans.roboticsautomat.,1994,10(1):65-71；即：不等式約束條件下的冗余機械臂軌跡規(guī)劃方法)：

為了高效地執(zhí)行重復(fù)運動任務(wù),y.zhang,引入重復(fù)運動指標(biāo)作為優(yōu)化準(zhǔn)則。形成重復(fù)運動規(guī)劃(repetitivemotionplanning,rmp)方案,使用二次規(guī)劃(qp)和遞歸神經(jīng)網(wǎng)絡(luò)(rnn)來進行冗余度解析(zhangy,wangj,xiay.adualneuralnetworkforredundancyresolutionofkinematicallyredundantmanipulatorssubjecttojointlimitsandjointvelocitylimits.ieeetransneuralnetw.,2003,14(3):658-667；即：基于關(guān)節(jié)角度和角速度限制的冗余機械臂軌跡規(guī)劃方法)。其中的重復(fù)運動指標(biāo)為如下漸近收斂性能指標(biāo)aoc(asympototically-convengentoptimalitycriterion)：

遞歸神經(jīng)網(wǎng)絡(luò)是求解基于二次型優(yōu)化描述的冗余度解析問題的有效方法。通常的神經(jīng)網(wǎng)絡(luò)求解器具有漸近收斂性能，在計算時間足夠長以后，能夠獲得有效解，且能應(yīng)用于動態(tài)時變優(yōu)化問題。

最近，具有有限時間收斂性能的遞歸神經(jīng)網(wǎng)絡(luò)被用求解時變問題。相比于具有漸近收斂動態(tài)特性的遞歸神經(jīng)網(wǎng)絡(luò)，終態(tài)收斂動態(tài)特性具有有限時間收斂性，不僅能夠改進收斂速度，而且達到較高收斂精度。然而，諸文獻中的有限時間收斂的神經(jīng)網(wǎng)絡(luò)都采用線性激勵函數(shù)，或具有無限值激勵函數(shù)，實際實現(xiàn)時，由于能量有限，無限值激勵函數(shù)神經(jīng)網(wǎng)絡(luò)實現(xiàn)時存在本質(zhì)困難。

技術(shù)實現(xiàn)要素：

為了克服現(xiàn)有冗余機械臂軌跡規(guī)劃方式的精度較低、收斂較慢、不易實現(xiàn)的不足，本發(fā)明提供一種精度較高、有限時間收斂、易于實現(xiàn)的基于終態(tài)吸引優(yōu)化指標(biāo)的冗余機械臂軌跡規(guī)劃方法；本發(fā)明的重復(fù)運動性能優(yōu)化指標(biāo)，

為了克服現(xiàn)有冗余機械臂軌跡規(guī)劃方式的精度較低、收斂較慢、不易實現(xiàn)的不足，本發(fā)明提供一種精度較高、有限時間收斂、易于實現(xiàn)的基于終態(tài)吸引優(yōu)化指標(biāo)的冗余機械臂軌跡規(guī)劃方法；本發(fā)明的重復(fù)運動性能優(yōu)化指標(biāo)，即終態(tài)吸引優(yōu)化指標(biāo)，形成冗余機械臂軌跡規(guī)劃的二次優(yōu)化方法。以具有有限值激活函數(shù)的終態(tài)神經(jīng)網(wǎng)絡(luò)作為求解器，在初始位置偏移情形下，實現(xiàn)冗余機械臂有限時間收斂的重復(fù)運動規(guī)劃任務(wù)。

為了實現(xiàn)上述目的，本發(fā)明提供如下的技術(shù)方案：

一種基于終態(tài)吸引優(yōu)化指標(biāo)的冗余機械臂軌跡規(guī)劃方法，包括以下步驟：

1)確定冗余機械臂末端執(zhí)行器期望目標(biāo)軌跡r^*(t)和期望回攏的關(guān)節(jié)角度θ^*(0)；

2)給定冗余機械臂實際運動時的初始關(guān)節(jié)角度θ(0),以θ(0)為運動起始點，將冗余機械臂重復(fù)運動規(guī)劃描述為二次規(guī)劃問題，其性能指標(biāo)為以下終態(tài)吸引的優(yōu)化指標(biāo)toc：

其中，0＜α＜1，sgn(·)為取符號函數(shù)，θ(t)-θ^*(0)表示各個關(guān)節(jié)角與初始期望關(guān)節(jié)角位移偏差，r^*表示機械臂末端執(zhí)行器期望的運動軌跡，表示末端執(zhí)行器期望的速度向量；由于機械臂的初始位置不在期望的軌跡上，通過減小末端執(zhí)行器期望路徑與實際運動軌跡位置間的誤差(r^*-f(θ))，改變末端執(zhí)行器的運動方向，βr＞0表示位置的參數(shù)增益，用來調(diào)節(jié)末端執(zhí)行器運動到期望路徑的速率；βθ＞0是一設(shè)計參數(shù)，用來形成關(guān)節(jié)位移的動態(tài)性能；j(θ)是冗余機械臂雅可比矩陣，f(θ)是冗余機械臂實際運動軌跡；

3)構(gòu)建有限值激活函數(shù)的終態(tài)神經(jīng)網(wǎng)絡(luò)模型，其動態(tài)特性由下述方程描述

其中，e為誤差變量，βe＞0為一設(shè)計參數(shù)，φ(·):r^n×n→r^n×n為嚴(yán)格單調(diào)遞增的激活函數(shù)，滿足φ(-·)＝-φ(·)；

求解步驟2)中的二次規(guī)劃問題，建立拉格朗日函數(shù)

式中，0＜α＜1，sgn(·)為取符號函數(shù)，λ(t)為拉格朗日乘子向量，λ^t是λ(t)向量的轉(zhuǎn)置；

通過拉格朗日函數(shù)對各個變量求導(dǎo)，并令其為零，得下述矩陣方程

wy＝v(6)

其中，

i為單位矩陣

記e＝wy-v，以式(5)所描述的有限值終態(tài)神經(jīng)網(wǎng)絡(luò)動態(tài)方程求解步驟3)中的矩陣方程(6)，其中，取

得到各關(guān)節(jié)角自運動軌跡。

本發(fā)明的技術(shù)構(gòu)思為：將冗余機械臂軌跡規(guī)劃的優(yōu)化指標(biāo)設(shè)計為一種終態(tài)吸引的優(yōu)化指標(biāo)toc(teminaloptimalitycriterion)，即

其中，θ、分別表示冗余機械臂的關(guān)節(jié)角度和角速度，θ^*(0)是各個關(guān)節(jié)角的期望初始值，0＜α＜1，βθ＞0是一設(shè)計參數(shù)，用來形成關(guān)節(jié)位移的動態(tài)性能，sgn(·)為取符號函數(shù)。

上述可重復(fù)運動優(yōu)化方案的設(shè)計思想是期望獲得下述動態(tài)方程

其中，關(guān)節(jié)角位移偏差ε(t)＝θ(t)-θ(0)。此動態(tài)方程所表達的系統(tǒng)有限時間收斂于零，需要的收斂時間t為

當(dāng)優(yōu)化的指標(biāo)函數(shù)達到最小值時，冗余機械臂的各個關(guān)節(jié)角可以回攏到期望的目標(biāo)軌跡上。

為了求解優(yōu)化指標(biāo)toc下的重復(fù)運動規(guī)劃問題，采用一種帶有限值激活函數(shù)的終態(tài)神經(jīng)網(wǎng)絡(luò)模型，該網(wǎng)絡(luò)的動態(tài)方程如下：

其中，βe＞0為一設(shè)計參數(shù)，e為動態(tài)方程的收斂誤差，φ(·):r^n×n→r^n×n為嚴(yán)格單調(diào)遞增的激活函數(shù)，滿足φ(-·)＝-φ(·)。在動態(tài)方程(5)中的激活函數(shù)φ(·)能量有界，即滿足

與現(xiàn)有的技術(shù)相比，本發(fā)明有如下優(yōu)點：

本發(fā)明提供一種終態(tài)吸引優(yōu)化指標(biāo)toc，在初始位置偏移情形下，實現(xiàn)冗余機械臂有限時間收斂的重復(fù)運動規(guī)劃任務(wù)。相比于已有的重復(fù)運動規(guī)劃方法，該方案具有更快的時間收斂特點，而且最終機械臂各個關(guān)節(jié)角的回攏精度更高。相比于具有漸近收斂遞歸神經(jīng)網(wǎng)絡(luò)，有限值終態(tài)神經(jīng)網(wǎng)絡(luò)具有有限時間收斂特性，它適于時變問題求解(式(6)為具有時變矩陣方程)，該神經(jīng)網(wǎng)絡(luò)求解器采用的激活函數(shù)是有限值激活函數(shù)，激活函數(shù)的能量有界，在工程應(yīng)用中易于實現(xiàn)，且成本低，符合工程實際需要。

附圖說明

圖1為本發(fā)明提供的重復(fù)規(guī)劃方案的流程圖。

圖2為取不同α值時的終態(tài)神經(jīng)網(wǎng)絡(luò)激活函數(shù)φ(·)。

圖3為采用本發(fā)明重復(fù)規(guī)劃方案的冗余機械臂puma560。

圖4為冗余機械臂puma560末端執(zhí)行器的運動軌跡。

圖5為冗余機械臂puma560的各個關(guān)節(jié)角軌跡。

圖6為冗余機械臂puma560各個關(guān)節(jié)角度和關(guān)節(jié)角速度。

圖7為冗余機械臂puma560末端執(zhí)行器的各位置誤差軌跡。

圖8為以有限值終態(tài)神經(jīng)網(wǎng)絡(luò)和遞歸神經(jīng)網(wǎng)絡(luò)求解時的誤差軌跡。

具體實施方式

下面結(jié)合附圖，對本發(fā)明做如下進一步描述。

參照圖1～圖8，一種基于終態(tài)吸引優(yōu)化指標(biāo)的冗余機械臂重復(fù)運動規(guī)劃方法，圖1為冗余機械臂重復(fù)運動規(guī)劃方案的流程，由以下3個步驟組成：1、確定冗余機械臂末端執(zhí)行器期望目標(biāo)軌跡和期望回攏各關(guān)節(jié)角度2、建立具有終態(tài)吸引優(yōu)化指標(biāo)的冗余機械臂重復(fù)運動二次規(guī)劃方案3、以有限值終態(tài)神經(jīng)網(wǎng)絡(luò)求解二次規(guī)劃問題，獲得各關(guān)節(jié)角軌跡。

1)確定期望軌跡

設(shè)定冗余機械臂puma560期望回攏的關(guān)節(jié)角度

θ^*(0)＝[0,0,0,0,0,0]^t

確定圓軌跡的圓心坐標(biāo)將圓的半徑設(shè)定為0.2m,其圓面與x軸的夾角為π/6rad，末端執(zhí)行器完成圓軌跡時間t＝10s。考慮到冗余機械臂puma560的初始位置可能不在期望的運動軌跡上，將機械臂的六個關(guān)節(jié)角度初值設(shè)為θ(0)＝[0,0,0,0,0+1,0]^t。

2)建立冗余機械臂重復(fù)運動的二次規(guī)劃方案

為實現(xiàn)冗余機械臂有限時間收斂的重復(fù)運動規(guī)劃，將冗余機械臂重復(fù)運動軌跡規(guī)劃描述為以下二次規(guī)劃問題，其終態(tài)吸引的優(yōu)化指標(biāo)為

其中，0＜α＜1，sgn(·)為取符號函數(shù)，θ、分別表示冗余機械臂的關(guān)節(jié)角度和關(guān)節(jié)角速度，θ^*(0)是各個關(guān)節(jié)角的期望初始值，βθ＞0是一設(shè)計參數(shù)，用來形成關(guān)節(jié)位移的動態(tài)性能。θ(t)-θ^*(0)表示各個關(guān)節(jié)角與初始期望關(guān)節(jié)角位移偏差，r^*表示機械臂末端執(zhí)行器期望的運動軌跡，表示末端執(zhí)行器期望的速度向量。由于機械臂的初始位置可能不在期望的軌跡上，通過減小末端執(zhí)行器期望路徑與實際運動軌跡位置間的誤差(r^*-f(θ))，改變末端執(zhí)行器的運動方向，βr＞0表示位置的參數(shù)增益，用來調(diào)節(jié)末端執(zhí)行器運動到期望路徑的速率。j(θ)是冗余機械臂雅可比矩陣，f(θ)是冗余機械臂實際運動軌跡。

3)以有限值終態(tài)神經(jīng)網(wǎng)絡(luò)求解上述二次規(guī)劃問題

構(gòu)建有限值激活函數(shù)的終態(tài)神經(jīng)網(wǎng)絡(luò)模型，其動態(tài)特性由下述方程描述

其中，e為誤差變量，βe＞0為一設(shè)計參數(shù)，φ(·):r^n×n→r^n×n為嚴(yán)格單調(diào)遞增的激活函數(shù)，滿足φ(-·)＝-φ(·)

為求解步驟2)中的二次規(guī)劃，建立拉格朗日函數(shù)：

式中，λ(t)為拉格朗日乘子向量，λ^t是λ(t)向量的轉(zhuǎn)置；

通過對拉格朗日函數(shù)各個變量求導(dǎo)，并令其為零，可得如下時變矩陣方程

wy＝v(6)

i為單位矩陣

其中，0＜α＜1，sgn(·)為取符號函數(shù)。

記e＝wy-v，以式(5)所描述的有限值終態(tài)神經(jīng)網(wǎng)絡(luò)動態(tài)方程求解步驟3)中的矩陣方程(6)，得到各關(guān)節(jié)角自運動軌跡。

圖2為取不同α值的終態(tài)神經(jīng)網(wǎng)絡(luò)激活函數(shù)，且0＜α＜1。從圖中可以看到隨著α的減小,有限值終態(tài)神經(jīng)網(wǎng)絡(luò)動態(tài)方程誤差收斂的速度會變快。

用于實現(xiàn)本發(fā)明重復(fù)規(guī)劃方案的冗余機械臂puma560如圖3所示。該機械臂由1個基座，3個連接桿構(gòu)成，通過關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4、關(guān)節(jié)5、和關(guān)節(jié)6組成。該冗余機械臂puma560連桿長度l＝[0.4318,0.4318,0.25625]^t米。

機械臂的末端執(zhí)行器在空間中的運動軌跡如圖4所示。圖中給出目標(biāo)圓軌跡及機械臂末端執(zhí)行器運動軌跡。可以看出，末端執(zhí)行器的初始位置不在期望的軌跡上。隨著時間的增加，實際軌跡和期望軌跡吻合，末端執(zhí)行器的終值位置誤差精度在三個方向xyz軸上達到10^-4，如圖7所示。

當(dāng)冗余機械臂puma560各關(guān)節(jié)角經(jīng)過10s后，各關(guān)節(jié)角的終值誤差達到10^-4，所有的關(guān)節(jié)軌跡基本閉合，其軌跡如圖5所示。

為了驗證終態(tài)吸引優(yōu)化指標(biāo)toc在重復(fù)運動規(guī)劃中的有效性，機械臂puma560末端執(zhí)行器完成圓軌跡過程中得到的關(guān)節(jié)角瞬態(tài)軌跡和角速度瞬態(tài)軌跡如圖6所示。從圖中可以看出，冗余機械臂的各關(guān)節(jié)角最終收斂于期望的關(guān)節(jié)角位置。當(dāng)t＝10s時，機械臂運動前后各關(guān)節(jié)角與其期望關(guān)節(jié)角位置之間的最大偏差為5.299*10^-5,用遞歸神經(jīng)網(wǎng)絡(luò)求解得到的各個關(guān)節(jié)角的終值誤差最大偏差為6.061*10^-4，如表1所示。

表1

為比較漸近收斂網(wǎng)絡(luò)與終態(tài)神經(jīng)網(wǎng)絡(luò)的收斂性能，定義計算誤差je(t)＝||w(t)y(t)-v(t)||2。圖8給出分別用有限值終態(tài)神經(jīng)網(wǎng)絡(luò)和遞歸神經(jīng)網(wǎng)絡(luò)求解二次規(guī)劃問題的誤差收斂軌跡。從圖中可以看出，以有限值終態(tài)神經(jīng)網(wǎng)絡(luò)求解時，當(dāng)時間t接近2.2秒時，誤差收斂精度已經(jīng)到達零附近,以遞歸神經(jīng)網(wǎng)絡(luò)求解時,誤差在5秒時還未收斂。