基于CPG的四足機器人穩定性優化及偏航補償

龍厚云，李 光，易 靜，薛晨慷，譚薪興，陳騰飛

（湖南工業大學 機械工程學院，湖南 株洲 412007）

0 引言

四足機器人有著高機動性和靈活性，可以適應不同地形的穩定行走。該優點可讓四足機器人執行復雜地形的救援巡邏等任務。

執行任務時，可能由于運動過程中的重心不穩定會引起四足機器人的機體偏航，嚴重情況下甚至引起機身翻轉，導致任務失敗。因此四足機器人的研發中迫切需要解決重心偏移引起機體運動不穩定問題。

近年來，和四足機器人的偏航有關的研究主要集中在重心位置、偏航補償等方面。如Lee等人為了解決四足機器人在重心偏移下trot步態的不穩定性問題，提出了一種基于遺傳算法（RCGA）調節重心投影點的位置的方法。結果顯示該方法可以連續、逐步地產生自適應足部運動。Wei等人為了提高四足機器人的運動性能，采用了一種對稱網絡拓撲結構設計的運動控制策略簡化了CPG的網絡結構，對四足機器人的步態規劃進行穩定控制。

韓寶玲等人使用基于PSO算法調整優化四足機器人機身橫向參數，優化后使四足機器人有更好的運動穩定性。易靜等人分析了trot步態下機體翻轉與行走偏航是由于四足機器人運動過程中機體重心偏移產生繞支撐對角線的翻轉力矩以及關節擺動時產生的反作用力矩引起。

于權偉等人提出一種改進麻雀搜索算法，實驗結果說明麻雀搜索算法具有高精度、收斂速度快、尋優效果明顯等優點。Wang等人利用最小二乘法估計參數的指數律、對數法和復合函數模型。并進行多種擬合優度檢驗，得到最優的復合函數，取得了良好效果。仿真證明最小二乘法擬合函數預測的準確性和實際可行性。

本文為了減少四足機器人行走時俯仰角與偏航量，需要對CPG控制器的主要參數使用改進麻雀搜索算法進行適應性優化。在優化的基礎上提出一種控制方法：用最小二乘法得出偏航函數模型預測出機體偏移量，用預測出的偏移量輸入CPG的旋轉步態再進行偏移量補償。結果顯示改進麻雀算法連續迭代相比原麻雀算法效果突出，對CPG運動控制模型參數優化后大大提高了機體穩定性。最小二乘法得出偏航函數可以實時控制偏航角補償。

1 模型建立

1.1 四足機器人模型

本次研究基于宇樹A1機器人，見圖1。簡化后的運動關節模型如圖2所示，運動關節包括：連接機身、控制髖關節側向運動的機身關節（Hip joint）；控制髖關節俯仰運動的大腿關節（Thigh joint）和控制膝關節運動的小腿關節（Calf joint）。四腿分別表示為左前腿（FL）、左后腿（RL）、右前腿（FR）和右后腿（RR）。包括足端坐標系和質心坐標系總共17個坐標系。在圖2中，以右后腿（RR）坐標系為例。四足機器人具體主要參數見表1。

圖1 機器人外形Fig.1 The appearance of the robot

表1 A1機器人重要參數Tab.1 Important parameters of A1

圖2 機器人關節模型Fig.2 Robot joint model

1.2 CPG網絡模型

四足動物普遍的行走方式是一種節律運動，通過控制每條腿相應關節的彼此協調完成運動。即按照一定的規律進行協調、控制、遵循簡單規律的周期行為動作，為低級神經中樞控制的一種自激行為。仿照自然界動物的低端神經中樞模式發生器控制四足機器人運動，這便是CPG控制機理。

Hopf振蕩器生成的信號的周期、幅值、收斂速度、相位等都有明確參數控制，在設計四足機器人運動網絡模型時，4個全連接對稱網絡拓撲結構的大腿關節與足內小腿關節皆容易建立耦合相。故CPG網絡模型采用Hopf振蕩器作為單元模型。將四足機器人運動的控制參數引入CPG，為了協調各足間相位關系，4個大腿關節之間相耦合，每條腿的大腿關節與小腿關節構成足內耦合。CPG運動控制數學模型為公式：

其中，x，y分別為大腿、小腿關節運動信號；θ和θ分別為經過相位耦合的大腿、小腿關節運動信號；調節在擺動相頻率（ω）和 支撐相頻率（ω）之間的變化速度；直接控制大腿關節運動幅值；控制收斂到極限環的速度；ω為步態周期；與支撐相占運動周期的大小相關，可控制4個大腿關節間的耦合相位，生成不同的步態；A直接控制小腿關節運動幅值。

CPG運動控制模型通過調節值生成多種運動步態，如靜步態（walk）、對角小跑步態（trot）、踱步態（pace）等多種運動步態。

trot步態具有較高的能效比，可以長時間保持一定的初速度，因而有非常廣泛的實際用途。由游洋威的trot步態研究分析可知，trot步態存在偏航現象。本文以trot步態為例進行優化，當相位差為0.5時為trot步態，預設參數見表2，其他參數見文獻［9］中生成的trot步態信號如圖3所示。

表2 CPG運動模型控制參數Tab.2 CPG motion model control parameters

圖3 CPG運動控制模型輸出trot步態各關節信號Fig.3 CPG motion control model outputs trot gait signals for each joint

1.3 四足機器人行走實驗分析

在Python環境的Pybullet仿真環境下，將CPG運動控制模型生成的trot步態信號導入四足機器人模型，取運動仿真時間為0 s與15 s時的一個步態運動周期過程圖像對比，如圖4、圖5所示。在圖4、圖5中，紅線為方向位移為0時的參考線。由圖4與圖5對比可知，0 s時參考紅線在四足機器人中心，15 s時已經與參考紅線有一定距離。對比表明偏航現象明顯，運動過程可見機體俯仰角起伏大，四足機器人運動穩定性需要進一步提高。

圖4 0 s時Pybullet仿真行走實驗Fig.3 Pybullet simulation walking experiment at 0 s

圖5 15 s時Pybullet仿真行走實驗Fig.4 Pybullet simulation walking experiment at 15 s

這里對行走數據變化進行分析，采集行走過程中60 s內的相關位移與姿態角數據：四足機器人在軸方向的行走位移、軸方向的距離、重心在軸的變化以及角和偏航角如圖6所示。

由圖6分析可知，重心起伏最大約有100 mm，角度約有6，證明機體重心起伏大、俯仰角明顯。偏航角值始終有規律地增加說明機體偏離預計軌道，機身也偏離原來方位。軸方向偏移量一直穩定增長則說明每個步態周期都有重心偏移。

圖6 行走實時位移與姿態角Fig.6 Real-time travel displacement and attitude angle

分析原因可知在四足機器人trot步態運動過程中，由于抬腿高度變化、重心與角起伏大等多個因素導致重心周期偏移。重心不在對角支撐線上時，重力會產生翻轉力矩，導致機體運動不穩定，嚴重時甚至引起機體翻轉。重心不在機體中心時，機體姿態會周期振蕩變化，表現出繞軸偏航。

四足機器人初始生成trot步態行走穩定性較差，是因為參數基于經驗預設，輸出參數信號復雜，僅通過經驗預設參數達不到機體穩定行走的效果。因此需要先對CPG網絡模型相應主要參數進行優化，尋找出CPG運動控制模型最優參數組合。并在優化、且提升了機體運動穩定性后，再進行偏航補償。

群優化算法應用日益廣泛，如唐鍇等人利用改進粒子群算法優化四足機器人機體尺寸及質心位置，Liu等人利用改進麻雀搜索算法解決了無人機路線規劃問題。Jia等人利用改進麻雀算法優化深度學習機等。本文提出一種新的改進麻雀搜索算法尋找CPG運動控制模型最優參數組合以提高機體運動穩定性。在Xue等人原有麻雀算法的基礎上，改進麻雀搜索算法有著較高搜索精度，全局和局部尋優能力更強。

2 改進麻雀搜索算法優化CPG參數

由上文研究可知四足機器人在行走偏航過程中，機身高度的實時變化與變化的速度都會影響機體重心位置。

2.1 基本麻雀搜索算法

麻雀搜索算法（SSA）是模擬麻雀群覓食過程中搜索食物與反捕食提出的智能優化算法。其種群有探索者、跟隨者與偵查者三部分。對此擬探討分述如下。

（1）探索者。搜索范圍廣，負責搜索最優值和引領種群。其位置更新公式如下：

（2）跟隨者。會跟隨最佳適應度值的探索者來搜索最優值。跟隨者的位置更新公式如下：

（3）偵查者。位置更新公式如下：

其中，為極小值；為步長控制參數；∈［-1，1］表示移動方向的隨機數；f為當前麻雀個體適應度值；f為當前全局最佳適應度值；f為當前全局最差適應度值。

2.2 改進自適應權重麻雀搜索算法

慣性權重對麻雀算法搜索最優值的影響很大。在［0，1］范圍內，慣性權重值大時算法全局搜索能力強，但不能在最優區域附近局部搜索最優值。慣性權重小，則搜索會陷入局部最優。算法要確保前期搜索范圍廣，后期局部搜索能力強。因此基于研究需要提出了一種動態非線性慣性權重公式如下：

其中，為最大迭代次數；為動態權重最大值；為動態權重最小值；為權重下降速度；為最大值與最小值持續時間?，F在取20，0.9，04，075，當迭代次數為30時，權重下降如圖7所示。

圖7 慣性權重和迭代次數關系曲線Fig.7 Relationship curve of inertial weight and iteration times

由圖7可知，迭代前期，權重衰減速度緩慢下降，有利于前期的全局搜索，快速向最優區域靠近，防止陷入局部最優；中、后期，權重快速下降，有利于搜索最優值的收斂。

2.3 仿真實驗

要尋優的主要參數為：為大腿關節擺動幅度，機體每次前進距離與大腿關節擺動幅值有關，決定了處于擺動相時騰空距離；ω直接控制四足機器人的運動周期，決定各關節運動速度。并且該值會決定重心位置移動的速度；A控制小腿關節的運動幅值，決定了小腿的抬腿高度，會直接影響騰空相的時間。以上主要運動控制參數隨不同環境有不同的最適應值。

仿真實驗中適應度函數取決于機體傳感器數據，研究選用的最佳適應度函數為：

其中，為四足機器人的行程；為不為零的極小值，以防分母為零；為四足機器人在該行程內偏航距離的最大值，加絕對值是為了防止往另一側偏航變為負值，拉低適應值；σ為軸重心的標準差。

2.4 結果分析

麻雀搜索算法與改進麻雀搜索算法的初始種群規模皆取為20，共迭代30次。圖8為優化后的四足機器人運動數據結果。

圖8 改進SSA算法優化后行走實時位移與姿態角Fig.8 Real-time travel displacement and attitude angle after improved SSA algorithm optimization

改進麻雀算法尋優后主要控制參數見表3，分別取前期未優化的種群和改進麻雀搜索算法優化后的種群，對其運動穩定性進行對比，結果見表4。

表3 改進麻雀搜索算法優化后運動參數Tab.3 Improved motion parameters after SSA optimization

表4 改進麻雀算法優化后CPG控制參數值Tab.4 Improvement of CPG control parameters before and after SSA optimization

改進麻雀搜索算法優化后四足機器人主要參數可使機體穩定性增加，角度上下波動降低一倍，軸方向偏航位移減少，重心起伏大幅度降低。實驗結果充分證明了改進麻雀算法對主要參數尋優的合理性和有效性。該方法極大提高了CPG運動模型的運動穩定性。

3 偏航補償

運動控制方法的實質是控制機器人的步態，使其按照一定的軌跡行走。運動控制方法是四足機器人的核心，直接決定了機器人的運動靈活性、穩定性和地形適應性。通過四足機器人的仿真實驗測試，對數據分析可以得到的增長數據與時間相關。圖8中，數值波動大，但很難從理論上求得與時間的函數。故使用最小二乘法擬合出偏航角平均增長函數，對四足機器人偏航進行補償控制。

3.1 最小二乘法擬合原理

引入一個近似的多項式函數（），使擬合值與實際值的誤差的平方和最小。誤差的平方和為：

在等式中：

非奇異，故方程組（8）的解存在唯一。

3.2 一階擬合偏航控制函數

在Python環境中的科學計算庫下進行矩陣運算，對原有值進行一階擬合可以得到平均增量函數如下：

其中，為時間，（）為偏航角平均增量。這里，給出了平均增量擬合函數（）與原數據的對比曲線繪制見圖9。

圖9 yaw平均增量擬合函數y（t）與原yaw數據對比Fig.9 Comparison of yaw mean incremental fitting function y（t）with the original yaw data

將的平均增量導入到CPG運動控制模型對應機身旋轉步態中，通過控制CPG模型的對應機身關節的運動，產生與重心偏移的翻滾力矩相反的力矩相互抵消，以此補償產生的偏航。對應機身關節的偏航補償函數（）為：

經過補償函數(t)控制仿真效果數據采集如圖10所示。

圖10 fy函數控制行走實時位移與姿態角Fig.10 fy function controls real-time travel displacement and attitude angle

由仿真實驗結果可知，重心偏移引起的偏航得到有效控制，產生的補償合理提高了四足機器人穩定性。軸方向的偏航位移可以穩定地控制為0。通過最小二乘法擬合得到的平均增量函數，有效地解決了軸方向偏航的問題。

4 結束語

本文基于CPG運動控制模型，探討研究四足機器人重心偏移產生的機體不穩定與偏航現象。利用改進麻雀算法優化對應參數使機體穩定性大幅度增加，并有針對性地就偏航優化問題展開研究。在此基礎上對偏航角進行分析，利用最小二乘法擬合出平均函數并對偏航角度加以預測，通過對應函數對應機身關節控制來補償偏航，仿真結果表明經過改進麻雀搜索算法優化與最小二乘法擬合函數能有效地控制偏航距離。采用該函數控制模型能夠及時補償偏航位移，使CPG運動控制模型對環境的穩定性和適應性得到了極大的提升。該方法也有一定的通用性，可用在其他步態優化與姿態角控制上。