江 沛，黃水華，韋 巍，單才華，項(xiàng) 基

（1.浙江大學(xué)電氣工程學(xué)院，浙江杭州310027；2.重慶大學(xué)機(jī)械傳動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，重慶400030；3.浙江正特集團(tuán)有限公司，浙江臺(tái)州317004）

帶關(guān)節(jié)約束的非冗余機(jī)械手臂二階逆運(yùn)動(dòng)學(xué)控制

江 沛1，2，黃水華1，韋 巍1，單才華3，項(xiàng) 基1

（1.浙江大學(xué)電氣工程學(xué)院，浙江杭州310027；2.重慶大學(xué)機(jī)械傳動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，重慶400030；3.浙江正特集團(tuán)有限公司，浙江臺(tái)州317004）

針對(duì)關(guān)節(jié)限位和關(guān)節(jié)速度約束下的非冗余機(jī)械手臂控制問(wèn)題，提出能夠同時(shí)滿足關(guān)節(jié)限位、關(guān)節(jié)速度約束的非冗余機(jī)械手臂二階逆運(yùn)動(dòng)學(xué)算法.該算法將關(guān)節(jié)限位約束轉(zhuǎn)換為帶有時(shí)變閾值的關(guān)節(jié)速度約束，將時(shí)變閾值關(guān)節(jié)速度約束由真實(shí)關(guān)節(jié)空間映射到修正關(guān)節(jié)空間，變?yōu)楣潭ㄩ撝档年P(guān)節(jié)速度約束；在修正關(guān)節(jié)空間通過(guò)加權(quán)最小二乘法，保證機(jī)械手臂運(yùn)動(dòng)過(guò)程中關(guān)節(jié)限位約束成立.該算法能夠通過(guò)自適應(yīng)調(diào)整權(quán)值矩陣滿足關(guān)節(jié)速度約束，通過(guò)定理證明了該算法能夠充分滿足關(guān)節(jié)限位、關(guān)節(jié)速度約束.在三關(guān)節(jié)非冗余機(jī)械手臂上進(jìn)行仿真實(shí)驗(yàn)，驗(yàn)證了該算法可以使閉環(huán)系統(tǒng)在實(shí)現(xiàn)軌跡跟蹤的過(guò)程中保證關(guān)節(jié)限位、關(guān)節(jié)速度約束成立.

非冗余機(jī)械手臂；二階逆運(yùn)動(dòng)學(xué)；關(guān)節(jié)限位約束；關(guān)節(jié)速度約束；修正關(guān)節(jié)；箝位任務(wù)

非冗余機(jī)械手臂是指關(guān)節(jié)數(shù)目等于機(jī)械手末端所在任務(wù)空間維度的機(jī)械手臂.由于非冗余機(jī)械手臂具有實(shí)現(xiàn)機(jī)械手臂末端任務(wù)的最小自由度，易于通過(guò)較小計(jì)算量實(shí)現(xiàn)較高精度的末端軌跡跟蹤的特點(diǎn)，廣泛應(yīng)用于工業(yè)生產(chǎn)中.

實(shí)現(xiàn)機(jī)械手臂末端對(duì)目標(biāo)軌跡跟蹤最典型的方法是利用雅克比矩陣的偽逆求解從任務(wù)空間到關(guān)節(jié)空間的逆運(yùn)動(dòng)學(xué)變換，從而生成適當(dāng)?shù)碾姍C(jī)運(yùn)動(dòng)指令[1-2].通過(guò)該方法可以在一階逆運(yùn)動(dòng)學(xué)層面求解關(guān)節(jié)指令速度[3-4]以及二階逆運(yùn)動(dòng)學(xué)求解關(guān)節(jié)指令加速度[5-8].

在機(jī)械手臂控制中通常需要考慮關(guān)節(jié)限位、關(guān)節(jié)速度等約束，若這些約束不能滿足，則有可能對(duì)機(jī)械手臂造成機(jī)械結(jié)構(gòu)上的損壞.在如何適度犧牲系統(tǒng)跟蹤精度以滿足機(jī)械手臂的關(guān)節(jié)約束方面，近20年取得了很多成果.Chan等[9]提出加權(quán)最小二乘法（WLN），通過(guò)改變關(guān)節(jié)權(quán)重，抑制相應(yīng)的關(guān)節(jié)運(yùn)動(dòng)，從而滿足關(guān)節(jié)限位約束.Huang等[10]提出在加權(quán)最小二乘法的基礎(chǔ)上，加入關(guān)節(jié)箝位任務(wù)，從而加速關(guān)節(jié)退出關(guān)節(jié)限位的箝位加權(quán)最小二乘法（CWLN）.Xiang等[11]提出變權(quán)重加權(quán)方法，在適當(dāng)犧牲跟蹤誤差的前提下，實(shí)現(xiàn)了關(guān)節(jié)限位避免及機(jī)械手臂避障.以上方法都是在一階逆運(yùn)動(dòng)學(xué)層面求解關(guān)節(jié)指令速度，難以滿足關(guān)節(jié)速度的約束.

Kircanski等[12]提出的算法可以在一階逆運(yùn)動(dòng)學(xué)層面通過(guò)減小任務(wù)空間的期望速度，保證關(guān)節(jié)指令速度滿足最大關(guān)節(jié)速度約束，而該算法可以推廣到二階逆運(yùn)動(dòng)學(xué)情形滿足最大關(guān)節(jié)加速度約束.該方法存在以下2個(gè)問(wèn)題：1）機(jī)械手臂不能在指定的時(shí)間內(nèi)完成對(duì)期望軌跡的跟蹤；2）機(jī)械手臂末端軌跡和期望的軌跡存在較大的誤差.Antonelli等[13]在此基礎(chǔ)上提出時(shí)間尺度概念，即在關(guān)節(jié)最大速度、最大加速度約束不滿足時(shí)，通過(guò)改變時(shí)間尺度，對(duì)任務(wù)空間軌跡在時(shí)間維度上進(jìn)行局部放縮，從而保證關(guān)節(jié)最大速度、加速度約束，并且實(shí)現(xiàn)對(duì)機(jī)械手臂末端期望軌跡的跟蹤.Bianco等[14-15]將變時(shí)間尺度算法拓展到高維約束的情形，并引入動(dòng)力學(xué)模型，解決了帶有力矩約束的逆運(yùn)動(dòng)學(xué)問(wèn)題.以上幾種方法均不能在指定時(shí)間內(nèi)實(shí)現(xiàn)對(duì)任務(wù)空間曲線的跟蹤，在二階（高階）逆運(yùn)動(dòng)學(xué)層面上時(shí)間尺度放縮只能滿足電機(jī)輸出能力有限造成的最大關(guān)節(jié)速度、加速度約束，難以滿足關(guān)節(jié)限位約束.

本文提出在關(guān)節(jié)限位及關(guān)節(jié)速度約束下的非冗余機(jī)械手臂二階逆運(yùn)動(dòng)學(xué)算法，求解關(guān)節(jié)指令加速度.由于加速度控制不易滿足關(guān)節(jié)限位約束，將關(guān)節(jié)限位約束轉(zhuǎn)換為帶有時(shí)變閾值的關(guān)節(jié)速度約束.時(shí)變閾值的逆運(yùn)動(dòng)學(xué)問(wèn)題無(wú)法采用加權(quán)最小二乘法求解，本文提出修正關(guān)節(jié)概念.通過(guò)時(shí)變非齊次變換，將關(guān)節(jié)空間映射到修正關(guān)節(jié)空間.在修正關(guān)節(jié)空間，時(shí)變閾值的關(guān)節(jié)速度約束轉(zhuǎn)換為定常閾值的修正關(guān)節(jié)速度約束，根據(jù)加權(quán)最小二乘法進(jìn)行求解.在此基礎(chǔ)上，對(duì)該控制算法進(jìn)行拓展，使之能夠同時(shí)滿足關(guān)節(jié)限位約束和關(guān)節(jié)速度約束，進(jìn)一步證明了在該算法下閉環(huán)系統(tǒng)滿足約束，最后通過(guò)仿真驗(yàn)證了該算法的有效性.

1 二階逆運(yùn)動(dòng)學(xué)控制

機(jī)械手臂正向運(yùn)動(dòng)學(xué)可以用如下形式表示：

式中：q為n維機(jī)械手臂關(guān)節(jié)位置矢量，x為末端執(zhí)行器位置在任務(wù)空間的m維矢量表示，對(duì)于非冗余機(jī)械手臂，m=n；f（·）為非線性正向運(yùn)動(dòng)學(xué)函數(shù).將式（1）對(duì)時(shí)間求導(dǎo)，可以得到一階運(yùn)動(dòng)學(xué)方程：

式中：J為雅克比矩陣，J=?f/?q.對(duì)時(shí)間求導(dǎo)，可得二階運(yùn)動(dòng)學(xué)方程如下：

已知執(zhí)行器末端期望位置xd、期望速度˙xd、期望加速度¨xd，求解對(duì)應(yīng)的關(guān)節(jié)加速度實(shí)現(xiàn)目標(biāo)跟蹤，可以通過(guò)基于偽逆的閉環(huán)逆運(yùn)動(dòng)學(xué)算法（CLIK）得到關(guān)節(jié)加速度指令：

式中：e為末端執(zhí)行器跟蹤誤差，e=x-xd；Ke、Kv為反饋增益矩陣，可以通過(guò)選擇適當(dāng)?shù)腒e、Kv，并引入誤差反饋使得e收斂.

2 帶關(guān)節(jié)限位約束的二階逆運(yùn)動(dòng)學(xué)控制

2.1 關(guān)節(jié)限位在關(guān)節(jié)速度層面的表述機(jī)械手臂物理關(guān)節(jié)限位可以表示為

式中：qi為機(jī)械手臂關(guān)節(jié)i的關(guān)節(jié)位置為機(jī)械手臂關(guān)節(jié)i的正、負(fù)限位.對(duì)于二階逆運(yùn)動(dòng)學(xué)控制，常常需要將關(guān)節(jié)位置層面的關(guān)節(jié)限位約束轉(zhuǎn)化到關(guān)節(jié)速度層面約束，本文給出連續(xù)系統(tǒng)關(guān)節(jié)限位約束在關(guān)節(jié)速度層面的表示如下：

式中：c為大于0的常數(shù)，˙qi為關(guān)節(jié)i的關(guān)節(jié)速度.為了探究式（5）、（6）之間的關(guān)系，給出如下定理.

定理1t0時(shí)刻，i=1，…，n且滿足-qi（t））≤（t）≤c（-qi（t）），t∈，t∈[t0，t1].

由定理1可知，在連續(xù)系統(tǒng)中，式（6）為約束（5）成立的充分條件.根據(jù)定理1，可以將機(jī)械手臂的關(guān)節(jié)限位約束（5）轉(zhuǎn)化為關(guān)節(jié)速度約束（6）.對(duì)于離散系統(tǒng)，定理1將不再成立，在離散系統(tǒng)中，關(guān)節(jié)限位約束將關(guān)節(jié)限位表示為增量形式：

式中：Δqi為機(jī)械手臂第i個(gè)關(guān)節(jié)在無(wú)窮小時(shí)間Δt內(nèi)關(guān)節(jié)位置的增量.將式（7）除以時(shí)間Δt，有

則有

其中c1=1/Δt.對(duì)于離散系統(tǒng)只需滿足c1≤1/T，則可以保證關(guān)節(jié)限位約束成立，且離散系統(tǒng)約束（9）與連續(xù)系統(tǒng)約束（6）具有相同的形式.

2.2 帶關(guān)節(jié)限位約束的二階逆運(yùn)動(dòng)學(xué)控制算法

關(guān)節(jié)速度約束（6）的閾值中包含關(guān)節(jié)位置變量q，為時(shí)變閾值的關(guān)節(jié)速度約束，無(wú)法采用WLN方法求解.需要尋找一個(gè)非齊次映射，將關(guān)節(jié)空間速度映射到該空間中，并使關(guān)節(jié)速度約束（6）在該空間的表達(dá)式為恒定閾值，從而采用WLN方法求解.本文提出修正關(guān)節(jié)概念，真實(shí)關(guān)節(jié)空間通過(guò)如下變換將真實(shí)關(guān)節(jié)空間映射到修正關(guān)節(jié)空間：

式中：ε1為關(guān)節(jié)避障區(qū)域?qū)挾龋?＞0.當(dāng)修正關(guān)節(jié)速度接近修正關(guān)節(jié)速度閾值時(shí)，通過(guò)減小相應(yīng)的關(guān)節(jié)權(quán)重，將抑制修正關(guān)節(jié)速度的進(jìn)一步變化.當(dāng)修正關(guān)節(jié)速度遠(yuǎn)離修正關(guān)節(jié)速度限位時(shí)，通過(guò)調(diào)整權(quán)重移除對(duì)相應(yīng)關(guān)節(jié)的運(yùn)動(dòng)限制，從而保證修正關(guān)節(jié)速度約束.

對(duì)式（10）進(jìn)行微分并代入式（13），可以得到真實(shí)關(guān)節(jié)空間閉環(huán)控制律：

將式（15）代入式（3），可得系統(tǒng)誤差方程：

系統(tǒng)誤差二階指數(shù)收斂.

對(duì)于非冗余機(jī)械手臂，當(dāng)關(guān)節(jié)限位觸發(fā)時(shí)，W1將會(huì)出現(xiàn)缺秩，在雅克比矩陣處于非奇異位型的情況下，仍然可能引入算法奇異，使JW1JT不滿秩，利用式（15）得到的關(guān)節(jié)加速度將趨于無(wú)窮大，超出電機(jī)的有效輸出范圍.為了解決機(jī)械手臂處于雅克比近奇異位型或算法近奇異位型，關(guān)節(jié)加速度過(guò)大的問(wèn)題，引入帶阻尼項(xiàng)的閉環(huán)控制律：

式中：λ為阻尼系數(shù)，λ≥0.較小的λ值系統(tǒng)有較高的準(zhǔn)確性，但在近奇異位型下魯棒性較差；較大的λ值系統(tǒng)有較好的魯棒性，但會(huì)增加系統(tǒng)誤差.本文使用Caccavale等[7]采用的方式設(shè)置阻尼項(xiàng)λ：

將式（10）代入時(shí)變閾值關(guān)節(jié)速度約束（6），可以得到修正關(guān)節(jié)速度約束：

當(dāng)真實(shí)關(guān)節(jié)在真實(shí)關(guān)節(jié)空間運(yùn)動(dòng)實(shí)現(xiàn)對(duì)期望軌跡的跟蹤時(shí)，相應(yīng)的修正關(guān)節(jié)在修正關(guān)節(jié)空間運(yùn)動(dòng)實(shí)現(xiàn)對(duì)相同軌跡的跟蹤.當(dāng)修正關(guān)節(jié)滿足約束（12），則在真實(shí)關(guān)節(jié)空間約束（6）成立.在真實(shí)關(guān)節(jié)空間中，帶有時(shí)變閾值關(guān)節(jié)速度約束（6）的逆運(yùn)動(dòng)學(xué)問(wèn)題被轉(zhuǎn)換為求解修正關(guān)節(jié)空間中帶有固定閾值關(guān)節(jié)速度約束（12）的逆運(yùn)動(dòng)學(xué)問(wèn)題.該問(wèn)題可以通過(guò)WLN方法進(jìn)行求解[9].閉環(huán)二階逆運(yùn)動(dòng)學(xué)解為

式中：W1∈Rn×n為對(duì)角權(quán)值矩陣.采用文獻(xiàn)[10]的方法設(shè)置加權(quán)矩陣如下：

式中：σmin為的最小奇異值，ò為奇異避障區(qū)域?qū)挾龋薽ax為最大阻尼常數(shù).

3 帶關(guān)節(jié)速度約束的二階逆運(yùn)動(dòng)學(xué)控制

關(guān)節(jié)速度約束可以表示為

式中：W2為對(duì)角加值矩陣，

其中，ε2為關(guān)節(jié)速度避障區(qū)域?qū)挾?利用式（20）得到的關(guān)節(jié)加速度由2部分組成.第一項(xiàng)是通過(guò)加權(quán)最小二乘法求得的相應(yīng)的關(guān)節(jié)加速度，當(dāng)關(guān)節(jié)速度接近關(guān)節(jié)速度閾值時(shí)，通過(guò)調(diào)整加權(quán)矩陣W2的值，使第一項(xiàng)關(guān)節(jié)加速逐漸趨向于0；當(dāng)關(guān)節(jié)速度遠(yuǎn)離速度閾值，則移除對(duì)關(guān)節(jié)速度變化的抑制.第二項(xiàng)加速度分量為-，其方向與當(dāng)前關(guān)節(jié)速度方向相反，將加速關(guān)節(jié)速度遠(yuǎn)離關(guān)節(jié)速度閾值，從而保證關(guān)節(jié)速度約束.

注解1式（17）為修正關(guān)節(jié)空間加權(quán)最小二乘法的二階逆運(yùn)動(dòng)學(xué)解，與CWLN方法的逆運(yùn)動(dòng)學(xué)解有相似的形式.在關(guān)節(jié)限位約束下，關(guān)節(jié)限位約束等價(jià)于具有時(shí)變閾值的等價(jià)關(guān)節(jié)速度約束（6），式（17）可以看作通過(guò)設(shè)置關(guān)節(jié)箝位任務(wù)-，保證真實(shí)關(guān)節(jié)速度達(dá)到c）時(shí)，真實(shí)關(guān)節(jié)速度具有和等價(jià)關(guān)節(jié)速度閾值相同的變化率.關(guān)節(jié)速度約束（19）中的速度閾值固定，式（20）可以看作一階CWLN方法在二階逆運(yùn)動(dòng)學(xué)上的推廣.式（20）中的關(guān)節(jié)箝位任務(wù)-的主要作用是加速關(guān)節(jié)速度離開(kāi)關(guān)節(jié)速度限位.

4 同時(shí)具有關(guān)節(jié)速度和關(guān)節(jié)限位約束的二階逆運(yùn)動(dòng)學(xué)控制

同時(shí)具有關(guān)節(jié)限位約束和關(guān)節(jié)速度約束的二階閉環(huán)逆運(yùn)動(dòng)學(xué)問(wèn)題可以描述為如下形式：

關(guān)節(jié)限位約束二階閉環(huán)逆運(yùn)動(dòng)學(xué)解（17）和關(guān)節(jié)速度約束的閉環(huán)逆運(yùn)動(dòng)學(xué)解（20）具有統(tǒng)一形式，通過(guò)適當(dāng)調(diào)整權(quán)值矩陣，可得同時(shí)滿足關(guān)節(jié)限位約束和關(guān)節(jié)速度約束的閉環(huán)系統(tǒng)關(guān)節(jié)加速度解：

式中：W為對(duì)角權(quán)值矩陣，其對(duì)角元素定義為W（i，i）=min（W1（i，i），W2（i，i））；i=1，…，n.

為了能夠在JW1JT缺秩的情況下得到可行關(guān)節(jié)加速度，在求解過(guò)程中加入了阻尼項(xiàng)，這可能導(dǎo)致約束不滿足.進(jìn)一步探討在控制律（25）下，閉環(huán)系統(tǒng)對(duì)約束的滿足情況.

定理2控制律（25）滿足機(jī)械手臂關(guān)節(jié)限位約束（23）和關(guān)節(jié)速度約束（24）.

式中：σmin、σmax分別為JW1/2的最小、最大奇異值.當(dāng)時(shí)，W2（i，i）=0，根據(jù)式（26），此時(shí)有＜0，與假設(shè)矛盾，故成立.同理可以證明，進(jìn)而可以證明.

注解2式（25）為關(guān)節(jié)限位和關(guān)節(jié)速度約束下的非冗余機(jī)械手臂二階逆運(yùn)動(dòng)學(xué)解.對(duì)于冗余機(jī)械手臂的二階逆運(yùn)動(dòng)學(xué)求解，由于冗余機(jī)械手臂存在雅克比矩陣零空間中的關(guān)節(jié)自運(yùn)動(dòng)，二階逆運(yùn)動(dòng)求解過(guò)程中通常出現(xiàn)解不穩(wěn)定的情況[5，17].為了使控制律穩(wěn)定，通常采用抑制雅克比零空間速度分量的方法[6]，而采用式（25）的加權(quán)方法無(wú)法完全實(shí)現(xiàn)對(duì)零空間速度分量的抑制，因而無(wú)法實(shí)現(xiàn)將其直接推廣的冗余機(jī)械手臂的情形.

注解3在實(shí)際應(yīng)用中，應(yīng)注意常數(shù)c數(shù)值的選取.較大的c值能夠在關(guān)節(jié)速度限位觸發(fā)時(shí)，加速關(guān)節(jié)速度遠(yuǎn)離速度約束閾值；但是較大的c值會(huì)在限位觸發(fā)時(shí)引入較大的誤差.

5 實(shí)驗(yàn)結(jié)果

為了驗(yàn)證控制算法滿足關(guān)節(jié)限位、關(guān)節(jié)速度約束的有效性，設(shè)計(jì)機(jī)械手臂在關(guān)節(jié)限位、關(guān)節(jié)加速度約束下跟蹤任務(wù)空間目標(biāo)曲線的仿真實(shí)驗(yàn).仿真實(shí)驗(yàn)以7關(guān)節(jié)乒乓球機(jī)器人為模型，由于研究對(duì)象為非冗余機(jī)械手臂，而任務(wù)空間曲線為三維空間曲線.取乒乓球機(jī)械臂靠近基座的3個(gè)旋轉(zhuǎn)關(guān)節(jié)串聯(lián)組成的非冗余機(jī)械臂，如圖1所示.圖中，Z1、Z2、Z3分別為關(guān)節(jié)1～3的旋轉(zhuǎn)軸.機(jī)械臂DH參數(shù)α、a、d如表1所示.關(guān)節(jié)限位qmin、qmax及關(guān)節(jié)速度閾值、如表2所示.機(jī)械臂初始關(guān)節(jié)位型q（0）= [2.174，2.426，3.042]T.機(jī)械臂末端在第三關(guān)節(jié)坐標(biāo)系下的位置坐標(biāo)為[100，180.9，0]T.系統(tǒng)采樣周期T=0.005 s.

首先采用式（4）求解關(guān)節(jié)加速，其中Kd=20，Kv=10.仿真結(jié)果如圖2、3所示.圖中，e為跟蹤誤差.關(guān)節(jié)運(yùn)動(dòng)曲線較平滑，系統(tǒng)跟蹤誤差小于0.08 mm.式（4）無(wú)法保證關(guān)節(jié)位置約束.當(dāng)t=5 s時(shí)，關(guān)節(jié)1位置超過(guò)正向關(guān)節(jié)限位.

表1 機(jī)械臂DH參數(shù)Tab.1 DH prameters of manipulator

表2 機(jī)械臂關(guān)節(jié)位置、速度閾值Tab.2 Joint position and velocity threshold of manipulator

圖1 機(jī)械臂仿真模型Fig.1 Manipulator simulation model

圖2 關(guān)節(jié)位置及關(guān)節(jié)速度曲線Fig.2 Joint position trajectories and joint velocity trajectories

為了在跟蹤任務(wù)空間軌跡的同時(shí)能夠滿足關(guān)節(jié)限位、關(guān)節(jié)速度約束，采用式（25）求解關(guān)節(jié)加速度，反饋增益系數(shù)設(shè)定同上.設(shè)定時(shí)間常數(shù)c=100≤1/T.關(guān)節(jié)避障區(qū)域?qū)挾瘸?shù)ε1=0.003，關(guān)節(jié)速度避障區(qū)域?qū)挾认禂?shù)ε2=0.15.采用文獻(xiàn)[10]的方法設(shè)置λmax，ò如下：

圖3 關(guān)節(jié)加速度曲線和跟蹤誤差Fig.3 Joint acceleration trajectory and tracking error

圖4 關(guān)節(jié)位置及關(guān)節(jié)速度曲線Fig.4 Joint position trajectories and joint velocity trajectories

仿真結(jié)果如圖4～6所示.圖5中，W1（1，1）、W2（1，1）分別為關(guān)節(jié)1的位置權(quán)重和速度權(quán)重.在[0，4.8]s時(shí)間段，關(guān)節(jié)位置和關(guān)節(jié)速度遠(yuǎn)離約束限位，加權(quán)矩陣W=I，λ=0.在t=4.8 s，真實(shí)關(guān)節(jié)1靠近正向限位，對(duì)應(yīng)的修正關(guān)節(jié)1的關(guān)節(jié)速度接近修正關(guān)節(jié)速度限位，如圖6（b）所示.通過(guò)式（14）減小修正關(guān)節(jié)1的權(quán)重系數(shù)，使修正關(guān)節(jié)加速度迅速減小到0，阻止突破修正關(guān)節(jié)速度閾值.此時(shí)，修正關(guān)節(jié)1對(duì)應(yīng)的真實(shí)關(guān)節(jié)速度被抑制到零，如圖6（c）所示，成功實(shí)現(xiàn)了真實(shí)關(guān)節(jié)的關(guān)節(jié)限位避障.隨著機(jī)械手臂的運(yùn)動(dòng)，1號(hào)關(guān)節(jié)速度反向并逐漸增大，當(dāng)t=6 s時(shí)實(shí)際關(guān)節(jié)1開(kāi)始反向退出關(guān)節(jié)正向限位，并在t=6.4 s時(shí)觸發(fā)負(fù)向速度約束，通過(guò)式（21）減小關(guān)節(jié)速度權(quán)重W2（1，1）；在t=6.9 s，W2（1，1）＜W1（1，1），關(guān)節(jié)速度約束成為抑制1號(hào)關(guān)節(jié)運(yùn)動(dòng)的主要因素.與關(guān)節(jié)位置約束相比，關(guān)節(jié)速度能夠更迅速地離開(kāi)速度閾值，這是由于關(guān)節(jié)限位約束是在修正關(guān)節(jié)空間通過(guò)加權(quán)最小二乘法實(shí)現(xiàn)的，而關(guān)節(jié)速度約束是在真實(shí)關(guān)節(jié)空間通過(guò)CWLN方法實(shí)現(xiàn)的.CWLN方法中的關(guān)節(jié)箝位任務(wù)可以加速推動(dòng)關(guān)節(jié)速度離開(kāi)關(guān)節(jié)約束閾值[10].

圖5 控制律（25）下的仿真結(jié)果Fig.5 Results of simulation under control law（25）

在關(guān)節(jié)限位、速度約束觸發(fā)過(guò)程中，JWJT出現(xiàn)缺秩，最小奇異值減小到3.07×10-4，小于ò.通過(guò)設(shè)定λ，如圖5（d）所示，保證了求解加速度有界，但阻尼項(xiàng)的加入使系統(tǒng)跟蹤誤差增大到15 mm.從t=7.8 s后，關(guān)節(jié)位置和關(guān)節(jié)速度均遠(yuǎn)離閾值，λ減小為0，跟蹤誤差逐漸減小到0.35 mm.

圖6 關(guān)節(jié)加速度曲線及修正關(guān)節(jié)運(yùn)動(dòng)曲線Fig.6 Joint acceleration trajectories and revised joint motion trajectories

仿真實(shí)驗(yàn)在Matlab環(huán)境下執(zhí)行，控制周期T=0.005 s，仿真任務(wù)持續(xù)時(shí)間為10 s.計(jì)算時(shí)間如表3所示.本文提出的控制律（25）與常用的基于偽逆的控制律（4）相比，未明顯增加運(yùn)算時(shí)間，控制周期的平均計(jì)算時(shí)間低于控制周期；在實(shí)際應(yīng)用中，計(jì)算效率高于Matlab，因此該控制律能夠運(yùn)用于實(shí)時(shí)性較高的應(yīng)用領(lǐng)域.

表3 曲線跟蹤仿真實(shí)驗(yàn)的計(jì)算時(shí)間Tab.3 Computing time of curve tracking simulation

6 結(jié) 語(yǔ)

本文提出能夠同時(shí)滿足關(guān)節(jié)限位、關(guān)節(jié)速度約束的非冗余機(jī)械手臂二階逆運(yùn)動(dòng)學(xué)算法.該算法將關(guān)節(jié)限位約束轉(zhuǎn)換為帶有時(shí)變閾值的關(guān)節(jié)速度約束，并將該約束由真實(shí)關(guān)節(jié)空間映射到修正關(guān)節(jié)空間，在修正關(guān)節(jié)空間通過(guò)加權(quán)最小二乘法保證關(guān)節(jié)限位約束；同時(shí)，能夠通過(guò)調(diào)整權(quán)值，在真實(shí)關(guān)節(jié)空間中保證關(guān)節(jié)速度約束.本文進(jìn)一步證明了采用該算法能夠使關(guān)節(jié)限位、關(guān)節(jié)速度約束充分滿足，并在三關(guān)節(jié)非冗余機(jī)械手臂上進(jìn)行仿真實(shí)驗(yàn).實(shí)驗(yàn)結(jié)果表明，采用該算法能夠在跟蹤目標(biāo)曲線的同時(shí)，滿足關(guān)節(jié)限位約束和關(guān)節(jié)速度約束，而較小的計(jì)算量使該算法能夠應(yīng)用于實(shí)時(shí)性較高的引用領(lǐng)域.

（

）：

[1]WHITNEYD E.Resolved motion rate control of manipulators and human prostheses[J].IEEE Transactions Human-Machine Systems，1969，10（2）：47-53.

[2]LIEGEOIS A.Automatic supervisory control of the configuration and behavior of multibody mechanisms[J].IEEE Transactions on Systems，Man and Cybernetics，1977，7（12）：868-871.

[3]NAKAMURA Y，HANAFUSA H.Optimal redundancy control of robot manipulators[J].The International Journal of Robotics Research，1987，6（1）：32-42.

[4]姜宏超，劉士榮，張波濤.六自由度模塊化機(jī)械臂的逆運(yùn)動(dòng)學(xué)分析[J].浙江大學(xué)學(xué)報(bào)：工學(xué)版，2010，44（7）：1348-1354.

JIANG Hong-chao，LIU Shi-rong，ZHANG Bo-tao.Inversekinematics analysis for 6 degree-of-freedom modular manipulator[J].Journal of Zhejiang University：Engineering Science，2010，44（7）：1348-1354.

[5]HOLLERBACH J M，KI S.Redundancy resolution of manipulators through torque optimization[J].IEEE Journal of Robotics and Automation，1987，3（4）：308-316.

[6]HSU P，MAUSER J，SASTRY S.Dynamic control of redundant manipulators[J].Journal of Robotic Systems，1989，6（2）：133-148.

[7]CACCAVALE F，CHIAVERINI S，SICILIANO B.Second-orderkinematic control of robot manipulators with Jacobian damped least-squares inverse：theory and experiments[J].IEEE/ASME Transactions on Mechatronics，1997，2（3）：188-194.

[8]GUO Dong-sheng，ZHANG Yu-nong.Acceleration-level inequality-based MAN scheme for obstacle avoidance of redundant robot manipulators[J].IEEE Transactions on Industrial Electronics，2014，61（12）：6903-6914.

[9]CHAN Tan-fung，DUBEY R V.A weighted least-norm solution based scheme for avoiding joint limits for redundant joint manipulators[J].IEEE Transactions on Robotics and Automation，1995，11（2）：286-292.

[10]HUANG Shui-hua，PENG Yong-gang，WEI Wei，et al.Clamping weighted least-norm method for the manipulatorkinematic control：avoiding joint limits [C]∥Proceedings of the 33th Chinese Control Conference.Nanjing：[s.n.]，2014：8309-8314.

[11]XIANG Ji，ZHONG Cong-wei，WEI Wei.A varied weights method for thekinematic control of redundant manipulators with multiple constraints[J].IEEE Transactions on Robotics，2012，28（2）：330-340.

[12]KIRCANSKI M，KIRCANSKI N.Resolved-rate and resolved-acceleration-based robot control in the presence of actuators’constraints[C]∥Proceedings of IEEE International Conference on Robotics and Automation.Albuquerque：IEEE，1997：235-240.

[13]ANTONELLI G，CHIAVERINI S，F(xiàn)USCO G.A new on-line algorithm for inversekinematics of robot manipulators ensuring path tracking capability under joint limits[J].IEEE Transactions on Robotics and Automation，2003，19（1）：162-167.

[14]BIANCO C G L，GERELLI O.Online trajectory scaling for manipulators subject to high-orderkinematic and dynamic constraints[J].IEEE Transactions on Robotics and Automation，2011，27（6）：1144-1152.

[15]BIANCO C G L，GHILARDELLI F.Real-time planner in the operational space for the automatic handling ofkinematic constraints[J].IEEE Transactions on Automation Science and Engineering，2014，11（3）：730-739.

[16]KHALIL H K，GRIZZLE J W.Nonlinear systems [M].Upper Saddle River：Prentice hall，2002.

[17]O’NEIL K A.Divergence of linear acceleration-based redundancy resolution schemes[J].IEEE Transactions on Robotics and Automation，2002，18（4）：625-631.

Second-order inversekinematic control method for non-redundant manipulator with joint constraints

JIANG Pei1，2，HUANG Shui-hua1，WEI Wei1，SHAN Cai-hua3，XIANG Ji1

（1.College of Electrical Engineering，Zhejiang University，Hangzhou 310027，China；2.State Key Laboratory of Mechanical Transmission，Chongqing University，Chongqing 400030，China；3.Zhejiang Zhengte Group Limited Company，Taizhou 317004，China）

A second-order inversekinematic control algorithm was proposed in order to control nonredundant manipulator with both joint limit and joint velocity constraints.The joint limit constraint was converted into the joint velocity constraint with time variant thresholds.Then real joint space was mapped into the revised joint space，in which joint limit constraint can be guaranteed by the weighted least-norm method.The method can achieve joint velocity constraint by adjusting the weight factors.A further proof that the both joint limit constraint and joint velocity constraint can be guaranteed by the method.A simulation was conducted on a three-link non-redundant manipulator in order to track a predefined trajectory.Results demonstrated that the method successfully guaranteed both joint limit constraint and joint velocity constraint.

non-redundant robot manipulator；second-order inversekinematic；joint limit constraint；joint velocity constraint；revised joint；clamping task

TP241

A

1008-973X（2015）10-1885-08

2014-09-04.浙江大學(xué)學(xué)報(bào)（工學(xué)版）網(wǎng)址：www.journals.zju.edu.cn/eng

國(guó)家自然科學(xué)基金資助項(xiàng)目（61374174）；中央高校基本科研業(yè)務(wù)費(fèi)專項(xiàng)資助項(xiàng)目（2013QNA4021）；新世紀(jì)優(yōu)秀人才資助項(xiàng)目（NCET-11-0459）；浙江省“錢江人才”計(jì)劃資助項(xiàng)目（2013R10047）；浙江省自然科學(xué)基金資助項(xiàng)目（LY13F030001）；杭州市產(chǎn)業(yè)鏈重大科技創(chuàng)新資助項(xiàng)目（20132111A04-2）.

江沛（1985—），男，博士生，從事機(jī)械手臂控制的研究.E-mail：denis-samurai@163.com

項(xiàng)基，男，教授，博導(dǎo).E-mail：jxiang@zju.edu.cn