摘 要：針對多項式插值軌跡規(guī)劃階次高、面對多個限制條件時優(yōu)化運算量大等問題，提出水下機器人關節(jié)空間基于改進粒子群優(yōu)化方法，設計以時間最優(yōu)為目標的5次多項式插值軌跡規(guī)劃算法。用Fluent求得腿部水阻力系數后，建立多個約束條件方程，通過更改搜索目標降低搜索維度，減少計算量。改進粒子群優(yōu)化采用可變權重，相較于標準粒子群優(yōu)化，改進粒子群優(yōu)化較好地平衡了全局收斂性和收斂速度。通過仿真實驗得到各關節(jié)的角度、角速度、角加速度曲線，驗證了所提出方法的可行性與有效性。

關鍵詞：水下機器人；軌跡規(guī)劃；粒子群優(yōu)化；多項式插值

中圖分類號：TP242.3 文獻標志碼：B 文章編號：1671-5276（2024）04-0208-06

Trajectory Planning of Underwater Robot Foot Based on Improved Particle Swarm Optimization

ZHI Chenghao， ZHAO Dongbiao

（College of Mechanical and Electrical Engineering， Nanjing University of Aeronautics and Astronautics， Nanjing 210016， China）

Abstract：Regarding the high order of polynomial interpolation trajectory planning and the large amount of optimization operations in the face of multiple constraints， a 5-time polynomial interpolation trajectory planning algorithm based on an improved particle swarm optimization method in the joint space of underwater robots with the goal of time optimality is proposed. With the leg water resistance coefficient calculated by Fluent， a number of constraint equations are established. The search target is changed to reduce the search dimension and calculation amount. Compared with standard particle swarm optimization， the improved particle swarm optimization is trade-off between global convergence and computation speed. The angle， angular velocity and angular acceleration curves of each joint are obtained by simulation experiments， which verifies the feasibility and effectiveness of the proposed method.

Keywords：underwater robot; trajectory planning; particle swarm optimization; polynomial interpolation

0 引言

相較于輪式、履帶式機器人，四足機器人在行走越障能力、環(huán)境適應能力上有較好的表現，因此在機器人領域是一個熱度較高的研究方向。四足機器人通過規(guī)劃行走步態(tài)和足端運動軌跡，可以在越障能力、工作效率和能量消耗上進行有效提升，從而在機器人攜帶有限電源的情況下，大大增加工作時間^[1]。尤其在水下工作環(huán)境中，水阻力使得行走消耗增高。因此，對四足機器人的行走步態(tài)和足端軌跡進行規(guī)劃和優(yōu)化是很有必要的。

在足端軌跡規(guī)劃方面，通常會采用各類軌跡函數進行軌跡、速度等各方面的規(guī)劃，如多項式插值、正弦法、NURBS（B樣條）曲線等。劉吉成等^[2]在輪腿混合式機器人研究中使用了修改后的復合擺線軌跡規(guī)劃，該方法實現了機器人足端曲線的平滑變換，解決了機器人足底與地面的滑動及拖地問題；李滿宏等^[3]為仿生六足機器人在不同路況下設計了拋物線、直線-拋物線組合軌跡，提高越障能力；柏龍等^[4]根據動物足端軌跡特性，選擇橢圓曲線作為足端運動軌跡，具有軌跡圓滑、無突變、導數連續(xù)的優(yōu)點；王立鵬等^[5]在研究液壓四足機器人中，根據復合擺線軌跡，改進為足端零沖擊軌跡規(guī)劃，并使用對角步態(tài)行走路；韓寶玲等^[6]在液壓四足機器人研究中，以Bézier曲線為基礎，通過改進步高和步長，來提高機器人工作過程中的平衡能力。

為了獲得更好的速度、加速度性能，通常做法是增加規(guī)劃軌跡函數的階次或復雜度，但這樣會增加軌跡優(yōu)化時的計算量。在機器人實際運行中有一些運動學、動力學約束，例如位置、速度、加速度、加加速度和轉矩的限制。為了提高效率，機器人應在滿足約束條件下盡可能快速地運動。但由于機器人動力學中的非線性和耦合，該最小時間控制問題可能計算復雜，解決困難^[7]。因此常會結合一些智能優(yōu)化算法對一個或多個指標求取最優(yōu)方案，如遺傳算法、粒子群算法（PSO）、人工魚群算法等以及各類混合算法。這些算法都是通過模擬某些自然現象和過程，或生物群體的智能行為，滿足一定條件下，在眾多方案或數值中尋找最優(yōu)值。相較于傳統(tǒng)數學推導優(yōu)化，這類優(yōu)化方法具有簡單、通用、便于并行處理等特點，而PSO只是通過內部速度進行更新，因此原理更簡單、參數更少、實現更容易^[8]。

標準粒子群優(yōu)化算法采用固定慣性權重，收斂速度較慢，且可能在局部極值收斂。本文提出基于改進PSO的時間最優(yōu)5次樣條插值的軌跡規(guī)劃方法，采用動態(tài)慣性權重，平衡收斂速度和全局收斂性，以軌跡運動時間為待優(yōu)化對象，將搜索空間降低到一維，從而不必推導多項式系數與優(yōu)化目標間復雜的函數關系，大幅降低了計算量和計算時間，并且結合水阻力仿真系數和建立的腿部運動模型，使得優(yōu)化結果限制在實際運動條件的范圍內，可靠性更高。最終在Matlab中模擬驗證優(yōu)化結果。

1 步態(tài)規(guī)劃

在研究四足機器人行走步態(tài)時，常會參考自然界四足動物的行走動作，其常用步態(tài)包括：行走、對角小跑、對側步、疾馳或跳躍。四足動物會根據行走的速度不同選擇不同步態(tài)^[9]，如圖 1所示。

行走步態(tài)速度最慢，行走過程中始終保持3只支撐足，1只擺動足，屬于靜態(tài)行走步態(tài)，需要保持質心投影在支撐三角內來保持穩(wěn)定。其余步態(tài)屬于動態(tài)步態(tài)，相較于行走步態(tài)，速度較快，依靠機器人自身質量慣性保持動態(tài)平衡。其中，對角步態(tài)保持對角線上的兩足保持同相，對側步態(tài)保持身體同側的兩足同相。疾馳或跳躍步態(tài)速度最快，都是以身體前后劃分為兩組規(guī)劃步態(tài)，但在同組足的相位差上有所差別。一般自然界中體型較大的動物使用疾馳步態(tài)，如馬，狗等，體型較小的動物使用跳躍步態(tài)，如兔等。

在研究四足機器人步態(tài)時，最常用的步態(tài)為對角小跑步態(tài)。該步態(tài)相較行走步態(tài)速度快，兩足支撐對角線通過質心，易于保持平衡，而其余步態(tài)均需要在動態(tài)行走過程中額外調整關節(jié)，如對側步態(tài)，質心會隨著步態(tài)相位變化往身體兩側來回變換，容易造成機器人側翻，需要調節(jié)髖關節(jié)進行姿態(tài)調整；疾馳或跳躍步態(tài)則在前后方向上質心有較大變化，落地沖擊力較大，需要肘關節(jié)、踝關節(jié)進行緩沖。

但本文機器人工作環(huán)境位于海水之下，易受到各個方向的亂流干擾，保證行走時的穩(wěn)定性至關重要，且相較于陸上的行走機器人，水下行走機器人還額外受到水阻力的影響，運動速度越快，受到水阻力越大。因此，選擇使用行走步態(tài)，四條腿按前左、后右、前右、后左的順序循環(huán)交替進入擺動相，每一條腿在擺動相中時間為1個完整行走周期的1/4，因此占空比為0.75，行走步態(tài)相位圖如圖 2所示。

2 水阻力系數仿真

2.1 物理模型的構建

通過SolidWorks軟件建立水下四足機器人原理樣機的二自由度簡化腿部模型如圖 3所示，測得其迎水面截面面積為18 301mm²，大腿長度L₁=0.12m，小腿長度L₂=0.10m，腿部處于伸直狀態(tài)。

將腿部模型導入Fluent的ICEM CFD中，創(chuàng)建計算域，使用非結構化網格。為了得到更好的模型細節(jié)，對腿部模型表面及周圍的流體進行網格加密，而遠離模型表面的流體劃分網格較大。

在Fluent中，設置海水密度ρ=1.05×10³kg/m³，動力黏度1.003×10^－3Ps·s。

2.2 腿部水動力性能分析

物體在流體中所受運動阻力的經驗公式為

式中：ρ為流體密度；S為流速垂直方向橫截面積；V為流體流速；C_d為水阻力系數。C_d的取值與流體性質、物體性質等一系列因素有關，不同情況下取值不同，因此需要進行仿真計算，為后面粒子群優(yōu)化設置受力限制提供計算基礎。

設定流域進口速度分別為0.25m/s、0.50m/s、0.75m/s、1.00m/s、1.25m/s，對腿部模型所受阻力進行模擬計算，最終Fluent根據海水密度、迎水面截面面積、腿部擺動速度，計算出不同速度下的阻力系數C_d，所得數據結果如表1所示。

將表1中所得的阻力與流速繪制成曲線，使用MATLAB進行多元非線性回歸擬合曲線，擬合曲線如圖 4所示，可得擬合公式為

F≈6.994 8V²（2）

結合水阻力的經驗公式、海水密度和橫截面積，可得水下機器人單個腿部模型的水阻力系數約為0.766。

殘差是指實際值與估計值之間的差，可以通過殘差來驗證回歸模型的準確性。根據數理統(tǒng)計，計算總平方和T_SS、殘差平方和R_SS后，最后求得可決系數R²：

計算得可決系數R²為0.999 8，接近于1，說明此回歸方程對實際值的擬合程度好，證明了其擬合的準確性高。

3 時間最優(yōu)軌跡規(guī)劃

3.1 機械腿運動學模型

為將機械腿在笛卡兒空間的運動轉換為關節(jié)空間的運動，方便對機械腿兩個關節(jié)進行控制，需要對機械腿建立坐標系，構建運動學關系與逆運動學求解。

首先對機械腿建立坐標系，如圖 5所示。對于關節(jié)i上的坐標系{i，規(guī)定z_i軸與關節(jié)軸i重合，x_i軸沿公垂線的指向，由關節(jié)軸線i指向關節(jié)軸線i+1。

將各個相鄰關節(jié)變換矩陣相乘，可得末端坐標系與基座標系的變換矩陣：

式中θ₁₂為θ₁+θ₂。

3.2 機械腿關節(jié)受力分析

所設計的水下機器人整體質量與浮力相近，因此忽略這方面的受力，只考慮腿部運動過程中受到的水阻力。水阻力方向與運動方向相反，因此連桿1、連桿2上受到的力與關節(jié)1、關節(jié)2引起的旋轉有關，如圖 6所示。

連桿2所受力矩τ₂可分解為兩關節(jié)旋轉引起的水阻力，而連桿1所受力矩τ₁可分解為關節(jié)1旋轉引起的水阻力以及關節(jié)2對連桿1的作用力。兩關節(jié)所受力矩計算公式如式（5）、式（6）所示。

最后可化簡為只與關節(jié)角θ₁、θ₂有關的函數，為后面粒子群優(yōu)化時提供關節(jié)受力的限制條件函數，防止受力超過電機能提供的最大力矩。

3.3 5次樣條插值函數構造

機械腿從初始位置運動至某個期望位置，一種方法是在路徑描述中給出一系列的期望中間點，機械腿必須經過中間點所描述的一系列過渡位置與姿態(tài)。通常期望機械腿的運動是平滑的，包括速度、加速度、脈動等的平滑，因此考慮采用在插值點處具有k－1階導數連續(xù)的k次樣條插值多項式。

5次樣條插值多項式的通式為

式中：e₀，…，e₅為5次多項式的系數；t為時間；h（t）為在對應時間t時機械腿所處的笛卡兒空間位置。

軌跡規(guī)劃分為兩種：1）在關節(jié)空間生成軌跡；2）在笛卡兒空間生成軌跡。本文選擇使用笛卡兒空間軌跡規(guī)劃，使生成的軌跡更加直觀。

因為本文機械腿是二自由度，運動軌跡只在x、y平面上，且設定軌跡關于時間中點對稱，因此只需在x、y兩個方向上分別進行5次樣條插值函數構造，且只用規(guī)劃一半軌跡，另一半進行對稱處理，即可獲得完整的運動軌跡。

完成5次插值軌跡規(guī)劃需要一定的約束條件：已知起始時刻t₀的位置x₀、速度v₀、加速度a₀，以及終末時刻t_f的位置x_f、速度v_f、加速度a_f。根據以上條件，可以推導出求解x、y方向的5次多項式系數的矩陣形式式（8）—式（10），式（10）即為5次插值函數的系數的解。

3.4 粒子群算法時間最優(yōu)求解

在選取粒子群優(yōu)化目標時，不同的選擇會導致計算復雜度和計算時間有很大的不同。時間t和式中的系數e都是待求解的未知數。若是將系數e作為自變量優(yōu)化，那么粒子群的粒子維數為6維，相比于1維的時間t，其計算量大且復雜。因此直接在時間t的一維搜索空間里優(yōu)化。

式（8）—式（10）是根據已知的初始末位置、速度、加速度條件，建立起軌跡運動時間T和5次插值多項式函數系數之間的關系映射。求解出優(yōu)化后的時間T，即可求得系數e。優(yōu)化目標函數及約束條件為：

式中：T為待優(yōu)化的這段軌跡的運行時間；θ·_i是第i個關節(jié)的角速度；?_i是第i個關節(jié)的角加速度；τ_i是第i個關節(jié)的所受力矩。設定最大角速度θ·_max=4rad/s，最大角加速度?_max=12rad/s²，最大關節(jié)受力矩τ_max=1.3N·m。

粒子群優(yōu)化算法是一種新的基于群體智能的優(yōu)化算法^[10]。它是一種模擬鳥群飛行覓食行為，并通過個體之間的協作來尋找最優(yōu)解的進化計算技術^[11]。

首先，粒子群算法在一維解空間中，給粒子總數為n個的粒子群，每個粒子只有一維屬性，隨機給定空間位置x_i和飛行速度v_i，并規(guī)定迭代次數上限M。粒子的適應度值由待優(yōu)化目標函數決定，該值用來衡量優(yōu)化的好壞程度。在迭代尋優(yōu)過程中，每個粒子通過跟蹤兩個極值來更新自己在解空間中的空間位置和飛行速度：個體極值p_i，即粒子本身在迭代過程中找到的最優(yōu)解粒子；全局極值p_g，即種群所有粒子在迭代過程中找到的最優(yōu)解粒子。

標準粒子群算法的迭代公式為

式中：v^k_ij為粒子i第k次迭代時飛行速度的第j維分量；x^k_ij為粒子i第k次迭代時空間位置的第j維分量；p^k_ij為粒子i第k次迭代時個體極值的第j維分量；p^k_gj為第k次迭代時為全局極值的第j維分量；w為慣性權重；c₁、c₂為學習因子；r₁、r₂為[0，1]范圍內的均勻隨機值。

適應度函數設為時間T，因為優(yōu)化目標為運動時間最短，所以適應度值越低，代表優(yōu)化結果越好。

fit（i）=T（15）

除了運動時間要盡可能小，還要考慮到約束條件式（12），將運動控制在合理的范圍內。在這方面，通常可采用罰函數調整超出約束條件的粒子適應度值，或者采用兩套適應度函數^[12]，在超出約束條件時切換，迫使其收斂到約束條件內。但這些都需要數次迭代，才有可能收斂約束內，效率不高。而分析約束條件和優(yōu)化目標可得，超出角速度、角加速度或力矩限制，都是由于時間T過小引起，因此可在超出約束時，限定該粒子往時間T增大的方向重新迭代至滿足約束條件的粒子位置。具體迭代步驟如下。

1）初始化粒子群，在搜索空間中隨機生成n個一維粒子，即n組時間T，并初始化粒子的位置x_i和速度v_i。

2）將n組時間T代入式（8）—式（10），求解出式（10）中5次插值函數的系數矩陣e。計算出各個軌跡的最大角速度、角加速度、力矩是否超出約束條件。若是，則將該粒子的位置x_i朝|v_i|隨機增加，增大時間T。

3）對每個粒子，用它的適應度值fit（i）與個體極值p_i和全局極值p_g比較。若是fit（i）更大，則更新p_i或p_g。

4）利用迭代公式（13）—式（14），更新粒子的位置x_i和速度v_i。

5）若到達迭代次數上限，則結束算法，并輸出優(yōu)化結果，否則返回步驟2）。

4 仿真結果分析

在式（13）中，等式右邊第1部分表示粒子第k次時刻的速度，保證算法的全局收斂性，右邊第2、3部分使算法具有局部收斂性。因此，式中慣性權重w是表示在多大程度上繼承原來的速度。

標準粒子群算法采用固定權重，通常根據經驗在0.8～1.2之間選取，雖然具有較快的收斂速度，但其后期容易陷入局部最優(yōu)，求解精度低。因此通常改進粒子群算法采用可變權重。一種采用較多的動態(tài)慣性權重值是線性遞減權重策略，其表達式如式（16）所示，而改進后的權重策略如式（17）所示。

式中：w_start、w_end分別為設定的慣性權重w的最大、最小值；k表示當前為第k次迭代；M為最大迭代次數。

在算法初期收斂速度稍慢，但在后期會提升局部搜索能力，從而更有可能找到全局最優(yōu)解。3種權重策略的權重值變化曲線如圖 7所示。

分別使用3種權重策略進行時間最優(yōu)軌跡優(yōu)化，過程中適應度值隨迭代次數變化曲線如圖 8所示?？梢钥闯觯潭嘀夭呗詫ふ易顑?yōu)值速率慢，直到第17次迭代才到達最優(yōu)值，且陷入局部最優(yōu)，求解精度低；最優(yōu)適應度值為0.795 2，明顯高于另兩種動態(tài)權重策略。線性權重策略較固定權重尋優(yōu)速度明顯提升，但依舊容易陷入局部最優(yōu)，在第16次迭代到達最優(yōu)，最優(yōu)適應度值為0.788 1。而改進權重策略，在前期維持了算法的全局搜索能力，后期收斂速度較快，提升了算法的局部尋優(yōu)能力，因此在尋優(yōu)速度以及最優(yōu)值上都有明顯的提升，在第4次迭代時就到達最優(yōu)值，且最優(yōu)適應度值為0.785 9，均優(yōu)于另兩種權重策略。

改進粒子群算法優(yōu)化過程中，以運動時間t為優(yōu)化目標，在約束條件范圍內，使得運動時間t盡可能小，其等效于使得各關節(jié)的平均角速度在不超出電機最大速率的情況下盡可能增大，以提高電機的使用率，各關節(jié)平均速度變化如圖9所示。從圖 9可看出，改進粒子群算法有效地提高了各關節(jié)的平均角速度。

圖10是經過改進粒子群優(yōu)化后，各關節(jié)在整段運動軌跡過程中的角度、角速度、角加速度以及所受力矩的變化。

從圖中可以看出整段軌跡中，運動平滑連續(xù)，沒有突然的脈動沖擊，且結合式設定的邊界條件可以看到角速度、關節(jié)受力矩均離邊界值有一定距離，而角加速度達到了邊界值?_2max=－12rad/s²，證明已達到在邊界條件范圍內的時間最優(yōu)解。因此，仿真驗證了該改進粒子群算法優(yōu)化的有效性。

5 結語

本文以所設計的水下四足機器人的二自由度機械腿為例，設定了關節(jié)角度、角速度、角加速度以及關節(jié)受力矩的約束條件，以時間最優(yōu)為目標，提出了在笛卡兒空間中的基于改進粒子群優(yōu)化的5次多項式插值軌跡規(guī)劃方法。

通過粒子群優(yōu)化算法，避免了多項式高階由此引起的優(yōu)化難度高、計算復雜的問題，并且以優(yōu)化目標空間為搜索空間，將粒子維度從6維降至1維，大大減少計算量，且采用改進權重策略后，相較于固定權重和線性權重，在優(yōu)化速率和優(yōu)化精度上有明顯的提升。最后通過仿真，得到平穩(wěn)無突變的運動軌跡，并且關節(jié)角加速度達到了約束條件的臨界值，說明已達到了約束條件內的最優(yōu)，驗證了優(yōu)化方法的可行性。

參考文獻：

[1] SAKAKIBARA Y，KAN K，HOSODA Y，et al. Foot trajectory for a quadruped walking machine[C]//IEEE International Workshop on Intelligent Robots and Systems，Towards a New Frontier of Applications. Ibaraki，Japan： IEEE，2002：315-322.

[2] 劉吉成，季洪超. 新型輪腿機器人步態(tài)規(guī)劃策略[J]. 中國機械工程， 2019， 30（18）： 2257-2262.

[3] 李滿宏，張建華， 張明路. 新型仿生六足機器人自由步態(tài)中足端軌跡規(guī)劃[J]. 中國機械工程， 2014， 25（6）： 821-825.

[4] 柏龍，龍樟，陳曉紅，等. 連續(xù)電驅動四足機器人腿部機構設計與分析[J]. 機器人，2018，40（2）：136-145.

[5] 王立鵬，王軍政，汪首坤，等. 基于足端軌跡規(guī)劃算法的液壓四足機器人步態(tài)控制策略[J]. 機械工程學報，2013，49（1）：39-44.

[6] 韓寶玲，朱琛，羅慶生，等. 液壓四足機器人關鍵步態(tài)參數優(yōu)化[J]. 北京理工大學學報， 2018， 38（10）： 1056-1060.

[7] CHOI Y K，PARK J H，KIM H S，et al. Optimal trajectory planning and sliding mode control for robots using evolution strategy[J]. Robotica，2000，18（4）：423-428.

[8] 董中華，陳鵬飛，李亞龍. 采摘機械臂的PSO-RBF神經網絡自適應控制[J]. 機械制造與自動化，2022，51（1）： 181-183，233.

[9] 崔越. 電驅四足機器人步態(tài)規(guī)劃及運動控制系統(tǒng)設計[D]. 馬鞍山：安徽工業(yè)大學，2019.

[10] SHI Y，EBERHART R. A modified particle swarm optimizer[C]//1998 IEEE International Conference on Evolutionary Computation Proceedings. IEEE World Congress on Computational Intelligence （Cat. No. 98TH8360）. Anchorage，AK，USA： IEEE，2002：69-73.

[11] 張頂學，關治洪，劉新芝. 一種動態(tài)改變慣性權重的自適應粒子群算法[J]. 控制與決策，2008，23（11）：1253-1257.

[12] 付榮，居鶴華. 基于粒子群優(yōu)化的時間最優(yōu)機械臂軌跡規(guī)劃算法[J]. 信息與控制，2011，40（6）：802-808.

收稿日期：2022-12-06