四足機器人抗重心偏移步態優化

黎晴亮，張志安，馬豪男，周何苗

南京理工大學 機械工程學院，南京 210094

科技的進步驅使著機器人技術的全面發展，對于足式機器人相關領域的研究越來越多[1-2]。傳統的移動機器人包括履帶式、輪式等，對地形要求較高，不適合崎嶇路面。而足式機器人由于其高度的靈活性和地形適應性，可以實現在各種不同地形情況下的穩定運動。足式機器人的這一優點讓其可以勝任復雜路面下的救援、運送物資等工作。

足式機器人在執行救援、物資運輸等任務時，載重會使機器人的重心產生偏移。重心的偏移會對足式機器人的運動穩定性產生影響，嚴重時甚至會導致步態紊亂、運動崩潰等嚴重問題。顯然，足式機器人具備較強的抗重心偏移能力對提高機器人的動態穩定性有重要作用。

近年來，對于四足機器人重心相關領域的研究多集中在規劃重心位置以及計算重心補償量上。文獻[3]提出了一種坡面條件下重心自適應控制算法，在機器人質心原有運動軌跡上加入橫向和縱向偏移補償量，并采用符號微分策略梯度法對質心偏移補償量進行自動調整，以減少機體翻轉扭矩從而提高機器人運動穩定性；文獻[4]提出了一種新的穩定性保證方法，通過模型分析法建立機器人的數學及物理模型，從理論角度分析了重心位置對機器人穩定性的影響，從而得出了較為合理的重心位置規劃區域，并通過仿真實驗驗證了改進重心位置規劃方法的有效性及合理性。以上對于四足機器人的研究方法依賴于理論分析，這種方法既過于復雜又摻雜眾多理想條件，導致結果難以令人滿意。與此同時，各類群智能優化算法在四足機器人領域應用頗多。文獻[5]提出了一種基于粒子群算法的四足機器人機身橫向調整參數優化方法，其以運動過程中機身的橫向調整參數為設計變量，目標函數綜合考慮了四足機器人軀體穩定性、行走直線性等運動性能，通過該優化算法使四足機器人能夠實現具有良好運動性能的靜步態；文獻[6]利用多目標遺傳算法優化CPG單元間的耦合系數矩陣，使得CPG網絡的輸出信號可以控制機器人關節按照一定的時序發生動作。粒子群算法[7]具有高效、簡潔的優點，可以避免復雜的理論建模，因此經常被用于復雜模型下的參數優化問題。本文提出了一種基于改進粒子群算法的抗重心偏移步態優化方法，通過CPG網絡控制模型在線生成機器人關節角度，并通過粒子群算法以機器人運動反饋信息為基礎不斷迭代，進而找出輸出模型的最佳參數組合，解決由于重心偏移帶來的穩定性問題。

1 模型建立

1.1 四足機器人模型

本文的研究對象為8自由度的四足機器人。為了簡化問題，不考慮髖關節的橫向自由度，即機器人每條腿均只包含髖關節和膝關節各一個旋轉自由度。該機器人前后腿運動模式都采用肘式結構，關節電機類型為舵機，僅需輸入位置期望信號即可，四足機器人整體線框圖如圖1所示。

圖1 四足機器人相框圖Fig.1 Wireframe of quadruped robot

對四足機器人單腿建立運動學模型如圖2所示。

該模型中，以四足機器人髖關節為原點建立笛卡爾坐標系，x正方向為機器人前進方向，y正方向垂直向上，l1和l2分別表示大腿和小腿長度，H為軀干距地面高度，S為步長，h為足端最大離地高度，α為大腿與y軸負方向夾角，β為大腿延長線與小腿之間的夾角，足端坐標為P(x p,y p)。由圖2通過幾何關系得到單腿逆運動學解算公式為：

圖2 單腿運動學模型Fig.2 Single leg kinematics model

同時以足端擺動相軌跡為擺線，支撐相軌跡為直線作為研究，研究該模型的單腿在兩個個完整運動周期里α和β的變化趨勢（取單腿步長S為40 mm，擺動相最大離地高度h為20 mm），如圖3所示。

圖3 髖關節、膝關節角度變化Fig.3 Changes in angle of hip and knee joints

1.2 Hopf振蕩器模型

對于足式機器人的控制，傳統方法是將其看成串、并聯機器人的模型。這種基于模型的控制方法，誤差會逐漸累積并導致不可逆轉的錯誤。近年來，一種基于仿生的控制策略——CPG理論越來越受到研究人員的重視[8-10]。CPG理論試圖仿照動物界中普遍存在的節律運動形式，如蜘蛛腿部的運動、蟑螂的行走、人類的呼吸等。CPG理論試圖通過建立各種數學模型，從而產生可調的、周期性震蕩的信號，使其能夠滿足節律運動的特點。

CPG的數學模型大致可分為兩類：一類是基于神經元模型；另一類是基于非線性振蕩器模型，這類方法以單個振蕩器為基本研究對象，并通過振蕩器自身輸出與相互耦合作用來產生可調的信號。常見的基于非線性振蕩器的CPG模型有Hopf模型[11-13]、Kuramoto模型等。其中Hopf振蕩器的數學模型為：

式中α、μ、β、ωsω和a均屬于模型參數，用于控制振蕩器輸出信號x和y。其中α控制振蕩器收斂到極限環的速度，μ表示振蕩器輸出信號的幅值，而β表示足式機器人步態運動中定義的負載因子，ωsω表示擺動相的頻率，a決定擺動相和支撐相之間切換的頻率。通過對這些參數進行合理的取值，再進行相應的數值變換，便可以使得x和y能夠作為足式機器人關節電機的相位控制信號。當β=0.25時單個Hopf振蕩器輸出信號如圖4所示。

圖4 Hopf振蕩器輸出Fig.4 Hopf oscillator output

觀察并對比圖3和圖4，可以發現圖4中Hopf振蕩器的輸出x和y，其變化趨勢近似于圖3中α和β的變化趨勢。對Hopf模型的參數做相應地調整，并對其輸出做一定的幅值、平移以及削波變化，可以盡可能地模擬實際情況下髖關節和膝關節的角度關系，整個過程的示意圖如圖5所示。本文的仿真實驗角度輸入采取的都是這種策略，目的是既能保證足端軌跡近似于擺線，提高四足機器人的動態穩定性，又能利用Hopf振蕩器模型化、便于構建控制系統的優點。

圖5 振蕩器的輸出適配足端軌跡過程Fig.5 Output of oscillator adapted to foot trajectory process

1.3 CPG網絡模型

對于四足機器人的控制，每個自由度需要一路輸入信號。不僅如此，在四足機器人的步態生成與切換中，不同關節之間的運動關系往往具有相互抑制、相互觸發的特性，這就要求各路關節電機的輸入信號之間具備一定的耦合性。針對本文中提到的8自由度四足機器人平臺，擬采用四路CPG信號來控制整個系統。其中，每個CPG單元控制單條腿，即Hopf振蕩器的x輸出作為髖關節的角度期望，而y輸出通過相應變換后作為膝關節的角度期望。同時通過一個耦合矩陣來表述各振蕩器之間的關系。多關節CPG網絡結構的拓撲結構如圖6所示，數學模型見公式（7）～（10）。

圖6 四足機器人CPG網絡拓撲結構Fig.6 CPG network topology of quadruped robot

其中，F為一個耦合系數矩陣，用來表示四路Hopf振蕩器總共8個輸出信號之間的耦合關系。根據上述微分方程組可知F為一個8階方陣，即：

其中，f ij表示第i項與第j項之間的連接權重，通過對f ij的合理賦值能夠調節各CPG單元輸出信號的相位關系，從而完成對整個四足機器人的關節協調控制。

2 四足機器人行走實驗

2.1 耦合系數矩陣F的賦值

最終的目的是得到具有一定耦合關系的四足機器人關節輸入信號，實現四足機器人四條腿的相位耦合。由于本文研究對象為8自由度四足機器人，可只考慮髖關節之間的耦合關系，膝關節輸入信號僅跟同路振蕩器的髖關節信號相關。根據以上分析可對耦合系數矩陣F做下列設定：

（1）任一膝關節信號對髖關節均沒有影響，因此矩陣F中的f12，f14，f16，f18，…，f72，f74，f76，f78均為0。

（2）膝關節僅僅受其所對應的腿的髖關節的影響，因此該矩陣F中第二行、第四行、第六行、第八行中的元素全為0。

（3）再排除自耦合的情況，因此該矩陣對角位置元素全為0。

根據上述原則，耦合系數矩陣變換為下面的一個稀疏矩陣，有效項共有12項。

即只需對F中剩余的12項進行合理的取值，便能夠調節CPG網絡輸出信號之間的相位關系，進而控制四足機器人產生相應的步態。下面以四足機器人中常見的trot步態為例，簡單說明如何設置耦合系數矩陣F的取值，來產生trot步態所需的關節電機位置輸入信號。

所謂“trot”步態，即四足機器人四條腿分成兩組，其中對角線上的兩組分為同一組，即FL和BR為一組，FR和BL為一組；同組之間兩腿同相位關系，不同組之間為異相關系，據此可確定F的值為：

2.2 仿真實驗

以walk步態為例，通過足端軌跡規劃、單足振蕩器模型參數適配（振蕩器其他相關參數參考文獻[14]）及CPG網絡模型的搭建，在MATLAB得到輸入信號的數值仿真結果如圖7所示。可以看到各關節變化趨勢符合walk步態。將得到的walk步態下的CPG網絡輸出信號應用于仿真實驗，四足機器人各項參數指標如表1所示，實際效果如圖8所示。為了更好地對機器人的行走穩定性做判定，本實驗在四足機器人軀干上增加了IMU節點，在足端增加了力傳感器節點，以便更好地收集四足機器人行走過程中的各項數據指標。機器人在平坦路面上行走時，其實時姿態角反饋如圖9所示。

圖7 Walk步態信號Fig.7 Walk gait signal

圖9 行走實驗中的實時姿態角Fig.9 Real-time attitude angle in walking experiment

表1 仿真模型各項參數Table 1 Simulation model parameters

圖8 Walk步態行走實驗Fig.8 Walk gait walking experiment

從圖8中可以看出，機器人行走狀態較為穩定：橫滾角roll以及俯仰角pitch均呈現微弱的周期震蕩的特點，而偏航角yaw的值始終變化不大，證明四足機器人在沿直線行走；而從足端力傳感器的反饋來看，以“左前→右后→右前→左后”的順序交替出現波峰，證明本文設計的CPG網絡模型能夠生成穩定有效的步態信號。

3 改進粒子群算法的步態優化

為了模擬實際運行過程中的重心偏移現象，在仿真平臺中將四足機器人的軀干重心向其前方平移L/8，右側平移H/8，示意圖如圖10所示。

圖10 俯視圖下的機器人重心偏移情況Fig.10 Offset of robot’s center of gravity in top view

利用控制變量的方法，僅僅在改變重心的情況下再次進行行走實驗，四足機器人姿態角如圖11所示。由圖可發現，四足機器人的姿態角roll、pitch的震蕩幅值明顯增加，偏航角yaw幅值大幅增加的同時也呈現出非周期性；同時發現機器人在x方向上的位移S X也大大下降。由此可得，重心偏移后的四足機器人在相同步態信號的驅動下，行走穩定性大大下降。

圖11 重心偏移后的實時姿態角Fig.11 Real-time attitude angle after center of gravity shifted

本文采用改進粒子群算法對CPG網絡模型中相關參數進行優化，進而解決由于重心偏移導致的行走穩定性問題。

3.1 粒子群算法的實現

結合生活實際可知，人負重一側腳步運動幅度和擺動相頻率均會變化。通過這一現象得到啟發，本實驗將四足機器人負重一側足端擺線軌跡的步長S、步高h以及擺動頻率ωsω這3個參數作為粒子群算法中的待優化變量；由于本實驗是MATLAB與webots聯合仿真，涉及到數據采集與輸入，基于減小工作量的考慮，種群規模不宜選擇過大，現將種群規模設為6。對待優化參數的范圍進行拉丁方抽樣獲得的種群初始位置如下：

仿真實驗中，四足機器人添加了眾多傳感器節點，包括：GPS、IMU等。因此，粒子群算法中的目標適應度函數設計主要依賴傳感器的數據。經過多次實驗，最終確定了目標適應度函數：

其中S表示四足機器人的在規定時間的行程；R y表示偏航角的極差；σr表示橫滾角的標準差；σp表示俯仰角的標準差。

粒子群算法的核心是：每次迭代過程中，根據式（11）來計算每個粒子的適應值，同時更新每個粒子的位置和速度，并從中找出個體最優解和種群最優解，更新公式如下：

式中，V id表示粒子的速度，X id表示粒子的當前位置，C1和C2分表表示局部加速系數和全局加速系數，Pid表示各單個粒子的歷史最佳位置，P gd表示整個種群的歷史最佳位置。

3.2 粒子群算法的改進

為了提高PSO算法的收斂速度，達到減少聯合仿真工作量的目的，本文設計了一種基于自適應調整權重[15-16]策略的優化方法。式（12）中權重ω代表粒子群算法的全局搜索能力，ω越大則全局搜索能力越強，ω越小則局部搜索能力越強。所謂自適應權重即每次迭代計算中的ω都會隨著粒子適應度值的改變而改變，有：

式中，ωmin和ωmax分別表示權重的最小值和最大值，fmax表示上輪迭代計算中粒子的最大適應度，fmin表示最小適應度值。自適應調整權重的策略為：上輪迭代中適應度值f較大的粒子時，相應地增大其權重ω，從而增強PSO的全局搜索能力，繼續搜索最優解；而隨著粒子適應度值f的減小，越來越接近最優解時，權重ω相應的減小，降低粒子的速度，從而增強PSO的局部搜索能力。

3.3 仿真實驗結果及分析

仿真實驗過程為：首先對種群進行初始化及更新，同時在MATLAB中構建CPG網絡模型并生成步態控制信號；然后導入Webots中的模型進行仿真；最后再對四足機器人所反饋的數據進行分析，并不斷進行迭代試驗直至各粒子都收斂到同一位置。實驗流程圖如圖12所示。

圖12 仿真迭代實驗流程圖Fig.12 Simulation iteration test flowchart

分別選取實驗前期未經優化的粒子和優化后的粒子，對其運動穩定性進行對比分析，結果如表2所示。

表2 優化前后機器人運動指標Table 2 Optimized robot movement indicators before and after

粒子群算法中各粒子的適應度值是對優化過程中四足機器人運動穩定性的表示。由式（12）可知，越小的適應度值對應的機器人穩定性越好。由表2可以看到，通過粒子群算法優化后，粒子的適應度值大大減小，同時除了前進距離略低于優化前的情況，其他指標均大幅優于優化前。這說明通過粒子群算法迭代搜索出的模型參數組合，能夠有效提高四足機器人的運動穩定性。

本實驗中粒子的適應度值變化趨勢如圖13所示。從圖13（a）圖可以看出，采用未經改進的粒子群算法時，各粒子適應度值總體上隨著迭代次數的增加而減小，在經過20次左右的迭代后各粒子的適應度幾乎不再變化，達到終止條件。而采用自適應調整權重策略的粒子群算法實驗結果如圖13（b）所示，可以看到經過改進后的算法在實驗結果相同的情況下，具有更快的收斂速度，大大減少了聯合仿真的時間，同時也證明本文提出的自適應調整權重策略的有效性。實驗后期四足機器人的姿態角較為穩定，直線行走能力也得到提高，這證明粒子群算法在解決多參數優化問題上的有效性。

圖13 粒子適應度值變化趨勢Fig.13 Change trend of particle fitness value

3.4 與多目標遺傳算法的對比

遺傳算法也常被用于多目標優化問題中。本節將提出的改進粒子群算法和多目標遺傳算法分別用于優化CPG網絡模型參數。

在遺傳算子設計中，變異算子和交叉算子選擇均勻變異和均勻交叉模式，選擇此模式有利于提高算法的全局搜索能力；選擇個體時采用最優個體保存法，即用適應度值最高的Nelite個個體替換適應度值最低的Nelite個個體。多目標遺傳算法相關參數設置見表3。為了更好地對比兩種群智能優化算法，讓多目標遺傳算法的種群規模、適應度函數與粒子群算法保持一致。

表3 遺傳算法參數設置Table 3 Genetic algorithm parameter setting

表4表示遺傳算法和改進粒子群算法多次試驗的結果。可以看出，對于相同的優化結果（最小適應度值），雖然遺傳算法多次實驗的迭代次數較為穩定，但其所需的迭代次數要高于改進粒子群算法，即其收斂速度不如改進粒子群算法；而改進粒子群算法雖然由于種群初始化導致收斂速度不一致，但總體效果好于遺傳算法。

表4 算法迭代次數比較Table 4 Comparison of algorithm iterations

4 結論

本文基于Hopf振蕩器構建了四足機器人的CPG控制網絡。通過單足軌跡規劃進而確定CPG數學模型的參數，以CPG網絡輸出曲線作為四足機器人的輸入信號，并進行機器人行走實驗證明了所設計CPG網絡的有效性；針對實際運行情況下機器人重心偏移導致的運動穩定性問題，利用粒子群算法通過多次迭代搜索運動模型的最佳參數，同時通過自適應調整權重的策略來加快算法的迭代速度，并通過與遺傳算法的比較驗證改進粒子群算法的有效性，解決了由于重心偏移帶來的穩定性問題。