宋孟軍 張明路

河北工業(yè)大學，天津，300130

0 引言

并聯(lián)機器人并聯(lián)機構(gòu)運動學正解的求解相對困難，且多數(shù)情況下正解的存在并不唯一，而并聯(lián)機器人并聯(lián)機構(gòu)運動學逆解的求解相對簡單，這與串聯(lián)機器人串聯(lián)機構(gòu)的求解恰好相反，但并聯(lián)機構(gòu)運動學正解和逆解的求解，相對于串聯(lián)機構(gòu)均要略顯復雜，尤其是并聯(lián)機構(gòu)運動學正解的求解。并聯(lián)機構(gòu)正解的求解暫無統(tǒng)一、有效且較為簡便的數(shù)值或幾何求 解方法［1－2］。趙杰等［3］利用幾何方法對Delta并聯(lián)機器人運動學正解進行了求解，可得到工作空間內(nèi)滿足運動連續(xù)性條件的唯一解；文獻［4－5］同樣利用代數(shù)消元和代數(shù)幾何方法對6－6型Stewart并聯(lián)機器人和五自由度并聯(lián)機構(gòu)進行了運動學正解求解。雖然并聯(lián)機構(gòu)的數(shù)值求解方法可以求解出并聯(lián)機構(gòu)的運動學正解精確解，但是計算過程繁瑣，計算結(jié)果變量多，存在多解擇優(yōu)等問題。幾何求解方法可以通過解算并聯(lián)機構(gòu)的幾何關(guān)系來較為簡捷地求解出并聯(lián)機構(gòu)的運動學正解，且正解唯一，但幾何求解方法一般應用于結(jié)構(gòu)較為特殊的并聯(lián)機構(gòu)，缺乏廣泛的適應性驗證。鑒于此，神經(jīng)網(wǎng)絡、遺傳算法、粒子群算法等控制學算法被引入到并聯(lián)機器人運動學正解的求解中。神經(jīng)網(wǎng)絡算法［6－7］可以避免復雜的公式推導，求解簡單，但需借助運動學逆解并利用逆解結(jié)果進行訓練學習；遺傳算法［8－9］能夠優(yōu)化出并聯(lián)機器人運動學正解的全部實數(shù)解，且穩(wěn)定性較好，但存在過早收斂、收斂速度緩慢等問題；粒子群算法［10－11］能夠進行全局優(yōu)化，且代碼易于實現(xiàn)，但容易陷入局部極值、存在進化后期收斂速度放慢等問題；

李曉磊等［12］于2002年提出了以覓食行為、聚群行為、追尾行為和隨機行為為特征的魚群算法，這是一種新型的集群智能算法，能夠加快收斂速度，防止局部停滯振蕩，并能夠跳出局部極值進行全局尋優(yōu)。在電力系統(tǒng)配電網(wǎng)規(guī)劃、水資源聯(lián)合調(diào)配、無線傳感網(wǎng)絡覆蓋優(yōu)化、圖像處理等領(lǐng)域［13－17］取得了良好的應用效果。

本文應用空間解析幾何方法求解出仿生移動機器人并聯(lián)機構(gòu)的解析解，并應用人工魚群算法對并聯(lián)機構(gòu)的運動學正解進行了求解，并對運動學正解結(jié)果進行了仿真驗證。

1 仿生移動機器人并聯(lián)機構(gòu)運動學正解

本文所研究對象為一種具有變形關(guān)節(jié)的新型多足移動機器人，通過變形關(guān)節(jié)可以實現(xiàn)多種運動模式，具有較強的地面適應能力，該移動機器人具有6條結(jié)構(gòu)相同的變形輪腿，所以又兼具輪式移動機器人和腿式移動機器人的特點，如圖1所示。變形移動機器人腿部為串聯(lián)機構(gòu)，具有4個旋轉(zhuǎn)自由度，依據(jù)仿生學原理而建立，多腿協(xié)調(diào)可以模仿多種動物進行運動，因此本文稱此種移動機器人為仿生移動機器人，此種機構(gòu)為仿生移動機構(gòu)。

圖1 多運動模式變形移動機器人腿部模型

與動物運動機理相同，仿生移動機器人在運動時存在擺動腿和支撐腿，擺動腿向前邁進的同時，支撐腿負責支撐并向前移動，此時，所有支撐腿可看作一個并聯(lián)機構(gòu)：軀體為動平臺，支撐物（如大地）為定平臺，并聯(lián)分支為各支撐腿。

由于3條支撐腿即可維持機體平衡，本文以對稱布置于仿生移動機器人機體兩側(cè)的3條機械腿為研究對象，作為仿生移動機器人并聯(lián)機構(gòu)的3個分支，由于每一分支均為虎克鉸—（變異）虎克鉸—球面副機構(gòu)，故3條支撐腿則構(gòu)成3－TTS并聯(lián)機構(gòu)。

1.1 空間機構(gòu)自由度

圖2為3－TTS并聯(lián)機構(gòu)的運動螺旋分布圖。將變異虎克鉸與虎克鉸進行數(shù)學建模，兩者具有相同的數(shù)學模型：兩轉(zhuǎn)動軸線在空間內(nèi)正交于一點。

圖2 仿生移動機器人并聯(lián)機構(gòu)的運動螺旋分布

圖2中，上三角代表動平臺，為仿生移動機器人機身，下三角代表定平臺，為地面，各支鏈的參考坐標系為OXYZ，固定于定平臺上，各支鏈內(nèi)的運動螺旋以＄ij表示（i代表第i個支鏈，j代表第j個運動副），在圖2內(nèi)的3個支鏈中選擇第一個支鏈的第一個運動螺旋與參考坐標系OXYZ的X軸重合，第二個運動螺旋與Y軸平行，第三個運動螺旋與Z軸平行，各關(guān)節(jié)點在參考坐標系OXYZ 中的坐標為：A（a1，0，0），B（a2，0，b2），C（a3，0，b3），D（a4，0，b4），則支鏈1中各運動螺旋在參考坐標系中可以表示為

計算式（1）的約束螺旋可以發(fā)現(xiàn)，支鏈1并不存在約束螺旋系。又因＄15和＄16兩運動副平行，所以存在一個冗余自由度。3個分支結(jié)構(gòu)相同，則共存在3個冗余自由度。

當動平臺在圖3所示位姿下發(fā)生有限的轉(zhuǎn)動或移動時，機構(gòu)的運動螺旋系為

圖3 仿生移動機器人并聯(lián)機構(gòu)的數(shù)學模型構(gòu)建

計算式（2）可以發(fā)現(xiàn)，此時支鏈1并不存在約束螺旋系，又因＄15與＄16相互平行，所以支鏈1仍存在一個冗余自由度，3個分支共存在3個冗余自由度。

考慮到圖2所示并聯(lián)機構(gòu)內(nèi)各支鏈自由度數(shù)均較多，所以在不影響仿生移動機器人穩(wěn)定性情況下，動平臺發(fā)生任意方向的運動，該并聯(lián)機構(gòu)都將具有6個自由度，即3個轉(zhuǎn)動和3個移動，并且包含3個冗余自由度，此并聯(lián)機構(gòu)共具有9個自由度。

另外，計算空間機構(gòu)自由度的常用公式還有Kutzbach Grubler公式：

式中，M 為機構(gòu)的自由度數(shù)；n為機構(gòu)內(nèi)桿件數(shù)；g為運動副的數(shù)目；fi為第i個運動副的自由度數(shù)。

因機構(gòu)內(nèi)公共約束的存在，所以式（3）通常寫為

式中，d為機構(gòu)的階數(shù)；λ為公共約束數(shù)。

由于圖3所示并聯(lián)機構(gòu)沒有公共約束，故同樣可以利用式（3）計算并得到相同的結(jié)果：

1.2 正解模型

3－TTS并聯(lián)機構(gòu)的三維模型及坐標系構(gòu)建情況如圖4所示。移動過程中，在所構(gòu)建的運動學模型基礎之上［18-19］，機器人動平臺的運動滿足零力矩點（ZMP）穩(wěn)定性判定理論［20］，可對機器人的運動軌跡進行規(guī)劃，并可同時對當前步態(tài)進行規(guī)劃，使得ZMP軌跡落在滿足穩(wěn)定裕度的穩(wěn)定區(qū)域內(nèi)，因此可假定A、E、F三支撐點處于瞬時穩(wěn)定支撐狀態(tài)，進而可開展機器人運動學正解的求解。

圖4 仿生移動機器人并聯(lián)機構(gòu)三維模型及坐標系構(gòu)建

如圖4所示，為便于計算，本文將采用相對運動的方法，將定平臺設定為機身，動平臺設定為地面。起始坐標系（Ox0y0z0）建立在3－TTS并聯(lián)機構(gòu)定平臺（機身）的幾何中心位置。動定平臺采用正三角形結(jié)構(gòu)，分別用于連接各分支第一關(guān)節(jié)和末端關(guān)節(jié)，并聯(lián)機構(gòu)的3個分支采用對稱分布，起始坐標系原點與各分支第一關(guān)節(jié)中心點連線的距離相等，夾角為120°。動平臺坐標系原點位于動平臺的外接球球心處，即位于圖4所示動平臺的幾何中心處。取AB、BC、CD 三桿長為90mm，定平臺外接球的半徑為70mm。

求解圖4所示下平臺O′點位置及下平臺姿態(tài)即為求解仿生移動機器人并聯(lián)機構(gòu)運動學正解問題。

首先，構(gòu)建圖4所示并聯(lián)機構(gòu)第一分支的運動學模型，如圖5所示。圖5所示運動學模型各變量及參數(shù)如表1所示。

圖5 仿生移動機器人腿部運動學模型構(gòu)建

表1 仿生移動機器人腿部連桿參數(shù)及關(guān)節(jié)轉(zhuǎn)角變量

依據(jù)坐標變換法則，可得關(guān)于表1所示各參數(shù)的變換矩陣，并求得關(guān)于該分支的運動學方程：

3－TTS并聯(lián)機構(gòu)其余兩分支運動學模型與第一分支的構(gòu)建方法相同，三分支末端位置的解析解如表2所示。

表2 仿生移動機器人并聯(lián)機構(gòu)三分支位置解

仿生移動機器人具有多種運動模式，可將3－TTS并聯(lián)機構(gòu)簡化為3－TRS并聯(lián)機構(gòu)，即依據(jù)變形關(guān)節(jié)的特點，將關(guān)節(jié)4（Cx4y4z4）固定于某特定運動模式下，保持不變。如圖3所示。

圖3為3個分支的4個轉(zhuǎn)角變量分別輸入?yún)⒖贾?、－π／12、－π／12、π時的數(shù)學模型構(gòu)建圖，因3個分支均為獨立的串聯(lián)機構(gòu)，本例以第一分支作為主動運動機構(gòu)，而其余兩分支作為從動運動機構(gòu)，即以主從移動機器人協(xié)同運動的方式進行3－TRS并聯(lián)機構(gòu)運動學正解求解。并以3個分支末端之間的距離為約束條件，建立位置正解的非線性約束方程組如下：

式（8）中，P1、P2、P3為圖3所示各分支末端的位置矢量，為已知矢量，P′1、P′2、P′3為運動后各分支末端的位置矢量，因第一分支為主動運動機構(gòu)，所以P′1為已知矢量，而P′2、P′3為未知矢量。結(jié)合已有的約束方程組，運用人工魚群算法的分解協(xié)調(diào)優(yōu)化思路，并進行一定地改進，可對仿生移動機器人并聯(lián)機構(gòu)的運動學正解進行求解。

2 算法設計

人工魚群算法通過模擬魚群的覓食、聚群、追尾和隨機游動等行為實現(xiàn)尋優(yōu)，具有并行搜索、快速收斂、快速跟蹤、算法簡單、全局性強等特點［12-13］。人工魚群的覓食行為是在視域范圍內(nèi)隨機移動一步，若食物濃度高于目前位置則向該方向前進一步，若嘗試多次后仍沒有找到合適位置，則執(zhí)行隨機行為。聚群行為是探索鄰域內(nèi)所有伙伴中心處的食物濃度，若高于目前位置且不擁擠則向該方向前進一步，否則執(zhí)行覓食行為。追尾行為是探索鄰域內(nèi)食物濃度最大的伙伴，若該伙伴處食物濃度高于目前位置且不擁擠，則向該方向前進一步，否則執(zhí)行覓食行為。隨機行為則是在視域范圍內(nèi)隨機移動一步。

隨著對象規(guī)模和問題復雜程度的增加，人工魚群算法也暴露出一些不足，例如，當對象數(shù)量多、尋優(yōu)域較大時，容易造成尋優(yōu)結(jié)果不收斂，或容易陷入局部極值，收斂效果不理想。本文將針對仿生移動機器人并聯(lián)機構(gòu)的特點對魚群算法的協(xié)調(diào)行為進行改進，尋找仿生移動機器人并聯(lián)機構(gòu)運動學正解的最優(yōu)解。

2.1 數(shù)學模型

本文目標是尋找滿足非線性約束方程組（式（4））的仿生移動機器人并聯(lián)機構(gòu)運動學正解的最優(yōu)解，即

式中，δ為擁擠度因子。

2.2 人工魚群初始化

式（9）內(nèi)，已知P′2、P′3兩未知矢量所在分支各有3個相互獨立的未知變量：

式中，rand（）產(chǎn)生0～1隨機數(shù)；i為魚群種類；j為每條人工魚的3個分量（第4個分量即θ5為常量）；k＝1，2，…，n，為人工魚數(shù)目標號；R 為尋優(yōu)域；AF（i）jk為人工魚群，如 AF（1）23表示第1種類魚群內(nèi)的第3條人工魚第2個分量（即原正解表達式內(nèi)的θ3）；θj為人工魚初始狀態(tài)。

2.3 人工魚群行為描述

人工魚的覓食、聚群和追尾行為成功均須向前移動一步，AF（i）k表示包含3條人工魚分量的第i組魚的第k條魚：

minAF（c）（）為第c類人工魚食物濃度，即為式（9）的距離誤差，c＝1，2，3。

人工魚AF（i）的覓食行為描述如下：

（1）引入人工魚AFk（i），設置嘗試搜索的最大次數(shù)（Try＿numb er）。

（2）在視域范圍內(nèi)任選兩條人工魚p：

（3）若 minAF（i）（AFp（i））＜ minAF（i）（AFk（i）），則AFk（i）＝AFp（i）；否則繼續(xù)步驟（2）。

（4）Try＿number次后，執(zhí)行隨機行為：

人工魚AF（i）的聚群行為描述如下：

（1）設定擁擠度因子δ、δ1、δ2；

（2）同時判定兩條魚鄰域內(nèi)的伙伴數(shù)目，t、k表達意義如前：

（3）滿足步驟（2）的伙伴數(shù)目n＿F：n＿F←n＿F＋1。

則兩條魚分別向中心位置前進一步：

否則執(zhí)行覓食行為。

人工魚AF（i）的追尾行為描述如下：

（1）從滿足｜AFt（i）－AFk（i）｜≤V內(nèi)的人工魚中，選 出 minAF（i）（AFk（i））最 小 的 人 工 魚k＝min，選擇過程內(nèi)，人工魚須同時滿足：

（2）將步驟（1）選出的最小值寫入公告板B（i）：

則兩條魚均向k＝min位置前進一步：

否則執(zhí)行覓食行為。

2.4 算法描述

人工魚群算法流程如圖6所示。

圖6 算法流程圖

（1）構(gòu)造數(shù)學模型，輸入人工魚初始狀態(tài)θj，設定視域V，步長S，擁擠度因子δ、δ1、δ2，尋優(yōu)域R。

（2）設定人工魚規(guī)模n、搜索次數(shù)Try＿num ber、初始化人工魚群FA（i）。

（3）初始化公告板B（c），B（c）為c類人工魚群的公告板：

（4）對每組兩條人工魚AF（i）k進行計算：

式（19）為協(xié)調(diào)函數(shù)。

（6）循環(huán)次數(shù)：n＿for←n＿for＋1。

（7）公告板：B（c）＝ minAF（c）。

（8）迭代次數(shù)：n＿while←n＿while＋1。

（9）將 每 次 迭 代 獲 取 的 公 告 板 B（c）輸出，結(jié)束。

尋優(yōu)域R與視域V 相同時，適當選取步長S，可獲得更高的食物濃度minAF（i）（）。

2.5 仿真實現(xiàn)

基于MATLAB仿真實現(xiàn)了本文仿生移動機器人并聯(lián)機構(gòu)運動學正解的求解。在如圖3所示仿生移動機器人并聯(lián)機構(gòu)數(shù)學模型上應用改進后的魚群算法，對并聯(lián)機構(gòu)運動學正解進行描述，結(jié)果如圖7～圖14所示。

從圖7、圖8可以看出，盡管迭代起始處距離誤差較大，但是隨著迭代開始，兩類魚群（AF（i））均能夠快速尋優(yōu)，因為魚群尋優(yōu)的目標是距離誤差最小化，食物濃度minAF（i）（）須趨近于0。

圖7 魚群1尋優(yōu)

圖8 魚群2尋優(yōu)

圖9表明，協(xié)調(diào)函數(shù)值能夠在擁擠度因子δ1、δ2規(guī)定范圍內(nèi)波動，因為AF（i）在尋優(yōu)過程中不僅考慮了兩類魚是否滿足同時尋優(yōu)，而且引入?yún)f(xié)調(diào)函數(shù)，從而使并聯(lián)機構(gòu)運動學正解不斷接近數(shù)學模型式（5）下的最優(yōu)。

圖9 協(xié)調(diào)尋優(yōu)

從圖10～圖13可以看出，除少數(shù)魚群聚集在局部極值之外，大多數(shù)魚群均能聚集于全局極值，這是因為AF（i）在聚群和追尾過程中充分考慮了擁擠度因子δ的選擇，在尋優(yōu)過程中同時滿足兩類魚群的視域、食物濃度和協(xié)調(diào)函數(shù)，使得兩類魚群逐漸向共同的優(yōu)化目標移動。

圖14表明，AF（i）所尋優(yōu)的結(jié)果可以達到仿生移動機器人并聯(lián)機構(gòu)運動學正解的求解精度要求，因為人工魚群AF（i）初始搜索范圍在尋優(yōu)域R內(nèi)，且視域V＝R，所以在步長S適當時，人工魚群AF（i）在覓食過程中增加了搜索全局極值的可能性，在聚群行為和追尾行為的作用下能夠跳出局部極值尋優(yōu)并加速向全局極值移動。

圖10 魚群1初始狀態(tài)

圖11 魚群1聚集

圖12 魚群2初始狀態(tài)

圖13 魚群2聚集

圖14 尋優(yōu)最終并聯(lián)機構(gòu)運動學正解

由圖3與圖14所示各分支末端位置解可知，魚群算法所求正解誤差與擁擠度因子的選取有關(guān)，本例中擁擠度因子δ、δ1、δ2分別為1.00、1.01、0.99。

如圖14所示，滿足式（5）的正解最優(yōu)解分別為：0.0074mm，0.0024mm，364.5556mm（初始值為363.5152mm），迭代次數(shù)為100次，所求最優(yōu)解可以滿足仿生移動機器人運動精度要求。

由圖7～圖14所示仿真結(jié)果可知，增加并聯(lián)機構(gòu)的分支數(shù)，例如由目前三分支增加至六分支，合理分布于機身兩側(cè)，并利用文中1.2節(jié)所述運動學正解模型構(gòu)建方法，對六分支并聯(lián)機構(gòu)的運動學正解模型進行構(gòu)建，即對六足仿生機器人的運動學正解模型進行構(gòu)建，通過增加式 （4）位置正解的非線性約束方程組數(shù)目，同理應用魚群算法可對六分支并聯(lián)機構(gòu)的運動學正解模型進行求解。

3 結(jié)論

（1）對一種多運動模式仿生移動機器人并聯(lián)機構(gòu)進行簡化，并運用螺旋理論計算該并聯(lián)機構(gòu)的自由度。

（2）應用坐標變換法則對仿生移動機器人并聯(lián)機構(gòu)的運動學正解模型進行構(gòu)建，可同時構(gòu)建仿生移動機器人并聯(lián)機構(gòu)的三維模型和該機構(gòu)運動學正解的數(shù)學模型。

（3）提出了基于尋優(yōu)域、視域、擁擠度因子等改進后的魚群算法，構(gòu)建了魚群算法的數(shù)學模型，引入了協(xié)調(diào)函數(shù)，對魚群算法進行描述。

（4）仿真實現(xiàn)魚群算法，結(jié)果表明，代碼易于實現(xiàn)，運用魚群算法對并聯(lián)機構(gòu)的運動學正解進行求解，無需逆解，無需初值，且具有較好的全局尋優(yōu)，快速尋優(yōu)性能。

［1］Jamwal P K，Xie S Q，Tsoi Y H，et al.Forward Kinematics Modelling of a Parallel Ankle Rehabilitation Robot Using Modified Fuzzy Inference［J］.Mechanism and Machine Theory，2010，45（11）：1537－1554.

［2］Li M，Huang T，Chetwynd D G，et al.Forward Position Analysis of the 3－DOF Module of the TriVariant：A 5－DOF Reconfigurable Hybrid Robot［J］.Journal of Mechanical Design，Transactions of the ASME，2006，128：319－322.

［3］趙杰，朱延河，蔡鶴皋.Delta型并聯(lián)機器人運動學正解幾何解法［J］.哈爾濱工業(yè)大學學報，2003，35（1）：25－27.

［4］Lee Tae－Young，Shim Jae－Kyung.Forward Kinematics of the General 6－6Stewart Platform Using Algebraic Elimination Original Research Article［J］.Mechanism and Machine Theory，2001，36（9）：1073－1085.

［5］Masouleh M T，Gosselin C，Husty M，et al.Forward Kinematic Problem of 5－RPUR Parallel Mechanisms （3T2R）with Identical Limb Structures［J］.Mechanism and Machine Theory，2011，46（7）：1－15.

［6］張世輝，孔令富，原福永，等.基于自構(gòu)形快速BP網(wǎng)絡的并聯(lián)機器人位置正解方法研究［J］.機器人，2004，26（4）：314－319.

［7］Parikh P J，Lam S S Y.A Hybrid Strategy to Solve the Forward Kinematics Problem in Parallel Manipulators［J］.IEEE Transactions on Robotics，2005，21（1）：18－25.

［8］Jamwal P K，Xie Shengquan，Kean C A.Kinematic Design Optimization of a Parallel Ankle Rehabilitation Robot Using Modified Genetic Algorithm［J］.Robotics and Autonomous Systems，2009，57（10）：1018－1027.

［9］賀利樂，劉宏昭，王朋，等.基于改進遺傳算法的六自由度并聯(lián)機器人位置正解研究［J］.應用科學學報，2005，23（5）：522－525.

［10］Montalvo I，Izauierdo J，Pérez R，et al.Particle Swarm Optimization Applied to the Design of Water Supply Systems［J］.Computers and Mathematics with Applications，2008，56（3）：769－776.

［11］車林仙，何兵，易建，等.對稱結(jié)構(gòu)Stewart機構(gòu)位置正解的改進粒子群算法［J］.農(nóng)業(yè)機械學報，2008，39（10）：158－163.

［12］李曉磊，路飛，田國會，等.組合優(yōu)化問題的人工魚群算法應用［J］.山東大學學報（工學版），2004，34（5）：64－67.

［13］Ravinda K，Ahuj A，Ozlem E，et al.A Survey of Very Large－scale Neighborhood Search Techniques［J］.Discrete Applied Mathematics，2002，123（1／3）：75－102.

［14］張秋亮.魚群算法在配電網(wǎng)規(guī)劃中的應用研究［D］.保定：華北電力大學（保定），2008.

［15］高玉芳，張展羽.混沌人工魚群算法及其在灌區(qū)優(yōu)化配水中的應用［J］.農(nóng)業(yè)工程學報，2007，23（6）：7－11.

［16］劉彥君.魚群算法及在無線傳感器網(wǎng)絡覆蓋優(yōu)化中的應用［D］.濟南：山東大學，2009.

［17］張赫，徐玉如，萬磊，等.水下退化圖像處理方法［J］.天津大學學報，2010，43（9）：827－833.

［18］Lacagnina M，Muscato G，Sinatra R.Kinematics，Dynamics and Control of a Hybrid Robot Wheeleg［J］.Robotics and Autonomous Systems，2003，45：161－180.

［19］Chen Li，Ma Shugen，Wang Yuechao，et al.Design and Modelling of a Snake Robot in Traveling Wave Locomotion［J］.Mechanism and Machine Theory，2007，42（12）：1632－1642.

［20］Ha Taesin，Choi Chong－Ho.An Effective Trajectory Generation Method for Bipedal Walking［J］.Robotics and Autonomous Systems，2007，55（10）：795－810.