仿生移動機器人并聯機構運動學正解的魚群算法求解

宋孟軍 張明路

河北工業大學，天津，300130

0 引言

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

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

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

1 仿生移動機器人并聯機構運動學正解

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

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

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

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

1.1 空間機構自由度

圖2為3－TTS并聯機構的運動螺旋分布圖。將變異虎克鉸與虎克鉸進行數學建模，兩者具有相同的數學模型：兩轉動軸線在空間內正交于一點。

圖2 仿生移動機器人并聯機構的運動螺旋分布

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

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

當動平臺在圖3所示位姿下發生有限的轉動或移動時，機構的運動螺旋系為

圖3 仿生移動機器人并聯機構的數學模型構建

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

考慮到圖2所示并聯機構內各支鏈自由度數均較多，所以在不影響仿生移動機器人穩定性情況下，動平臺發生任意方向的運動，該并聯機構都將具有6個自由度，即3個轉動和3個移動，并且包含3個冗余自由度，此并聯機構共具有9個自由度。

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

式中，M 為機構的自由度數；n為機構內桿件數；g為運動副的數目；fi為第i個運動副的自由度數。

因機構內公共約束的存在，所以式（3）通常寫為

式中，d為機構的階數；λ為公共約束數。

由于圖3所示并聯機構沒有公共約束，故同樣可以利用式（3）計算并得到相同的結果：

1.2 正解模型

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

圖4 仿生移動機器人并聯機構三維模型及坐標系構建

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

求解圖4所示下平臺O′點位置及下平臺姿態即為求解仿生移動機器人并聯機構運動學正解問題。

首先，構建圖4所示并聯機構第一分支的運動學模型，如圖5所示。圖5所示運動學模型各變量及參數如表1所示。

圖5 仿生移動機器人腿部運動學模型構建

表1 仿生移動機器人腿部連桿參數及關節轉角變量

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

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

表2 仿生移動機器人并聯機構三分支位置解

仿生移動機器人具有多種運動模式，可將3－TTS并聯機構簡化為3－TRS并聯機構，即依據變形關節的特點，將關節4（Cx4y4z4）固定于某特定運動模式下，保持不變。如圖3所示。

圖3為3個分支的4個轉角變量分別輸入參考值0、－π／12、－π／12、π時的數學模型構建圖，因3個分支均為獨立的串聯機構，本例以第一分支作為主動運動機構，而其余兩分支作為從動運動機構，即以主從移動機器人協同運動的方式進行3－TRS并聯機構運動學正解求解。并以3個分支末端之間的距離為約束條件，建立位置正解的非線性約束方程組如下：

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

2 算法設計

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

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

2.1 數學模型

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

式中，δ為擁擠度因子。

2.2 人工魚群初始化

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

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

2.3 人工魚群行為描述

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

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

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

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

（2）在視域范圍內任選兩條人工魚p：

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

（4）Try＿number次后，執行隨機行為：

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

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

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

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

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

否則執行覓食行為。

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

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

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

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

否則執行覓食行為。

2.4 算法描述

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

圖6 算法流程圖

（1）構造數學模型，輸入人工魚初始狀態θj，設定視域V，步長S，擁擠度因子δ、δ1、δ2，尋優域R。

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

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

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

式（19）為協調函數。

（6）循環次數：n＿for←n＿for＋1。

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

（8）迭代次數：n＿while←n＿while＋1。

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

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

2.5 仿真實現

基于MATLAB仿真實現了本文仿生移動機器人并聯機構運動學正解的求解。在如圖3所示仿生移動機器人并聯機構數學模型上應用改進后的魚群算法，對并聯機構運動學正解進行描述，結果如圖7～圖14所示。

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

圖7 魚群1尋優

圖8 魚群2尋優

圖9表明，協調函數值能夠在擁擠度因子δ1、δ2規定范圍內波動，因為AF（i）在尋優過程中不僅考慮了兩類魚是否滿足同時尋優，而且引入協調函數，從而使并聯機構運動學正解不斷接近數學模型式（5）下的最優。

圖9 協調尋優

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

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

圖10 魚群1初始狀態

圖11 魚群1聚集

圖12 魚群2初始狀態

圖13 魚群2聚集

圖14 尋優最終并聯機構運動學正解

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

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

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

3 結論

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

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

（3）提出了基于尋優域、視域、擁擠度因子等改進后的魚群算法，構建了魚群算法的數學模型，引入了協調函數，對魚群算法進行描述。

（4）仿真實現魚群算法，結果表明，代碼易于實現，運用魚群算法對并聯機構的運動學正解進行求解，無需逆解，無需初值，且具有較好的全局尋優，快速尋優性能。

［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型并聯機器人運動學正解幾何解法［J］.哈爾濱工業大學學報，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］張世輝，孔令富，原福永，等.基于自構形快速BP網絡的并聯機器人位置正解方法研究［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］賀利樂，劉宏昭，王朋，等.基于改進遺傳算法的六自由度并聯機器人位置正解研究［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］車林仙，何兵，易建，等.對稱結構Stewart機構位置正解的改進粒子群算法［J］.農業機械學報，2008，39（10）：158－163.

［12］李曉磊，路飛，田國會，等.組合優化問題的人工魚群算法應用［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］張秋亮.魚群算法在配電網規劃中的應用研究［D］.保定：華北電力大學（保定），2008.

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

［16］劉彥君.魚群算法及在無線傳感器網絡覆蓋優化中的應用［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.