基于改進CMA-ES 的雙足機器人踢球算法設(shè)計*

周鼎宇 梁志偉

（南京郵電大學(xué)自動化與人工智能學(xué)院 南京 210023）

1 引言

RoboCup 作為足球機器人的重要比賽平臺之一，極大地促進了人工智能和智能機器人學(xué)科的發(fā)展。其中，標準平臺組（Standard Platform League）的研究包括多仿人機器人的分布式管理、運動規(guī)劃和路徑規(guī)劃等方面［1～2］。由于實時動態(tài)環(huán)境下雙足機器人對運動加速度的變化很敏感，運動狀態(tài)的改變極易影響自身的平衡，引起機器人的倒地。所以踢球動作的優(yōu)劣是贏得比賽的關(guān)鍵技術(shù)之一。

目前國內(nèi)外對此已經(jīng)有了較為成熟的研究，美國德州大學(xué)的UT隊設(shè)計了一種基于黑盒優(yōu)化算法的踢球方法［3］，通過利用機器學(xué)習(xí)對踢球的距離和速度進行優(yōu)化，目前在RoboCup3D仿真組中得到了很好的效果；葡萄牙的HTWK團隊使用關(guān)鍵幀插入法［4～5］，通過預(yù)先設(shè)定的關(guān)節(jié)角度集合執(zhí)行踢球動作，但這種方法必須等到所有關(guān)鍵幀結(jié)束才可以執(zhí)行下一個動作，在實際中效果并不連貫；德國B-Human 團隊開發(fā)了一種基于倒立擺模型的在線踢球軌跡生成方法［6］，利用ZMP判據(jù)有效提高了踢球時的穩(wěn)定性，但執(zhí)行起來速度較慢并且踢球距離較短。

為了保證機器人可以穩(wěn)定有效地進行踢球動作，本文主要采用基于改進的協(xié)方差自適應(yīng)進化策略（CMA-ES）算法的足球機器人射門運動規(guī)劃規(guī)劃最優(yōu)的踢球路徑以及腿部軌跡。并將該方法應(yīng)用于標準平臺組的實際比賽中驗證該方法的有效性和局限性。

2 改進的CMA-ES算法

2.1 CMA-ES算法基本原理

CMA-ES［7］屬于分布式估計算法（EDA）的一種，是一種演化策劃算法。它是基于統(tǒng)計的方式使用多元高斯分布在問題優(yōu)化的解空間中采樣，并通過適應(yīng)度函數(shù)來評估每個采樣參數(shù)，完成所有候選采樣集的評估后，下一代采樣集的生成會偏向上一代成功的搜索方向。通過采樣、更新等的多次迭代完成優(yōu)化過程。

CMA-ES 算法的每代進化都從多元正態(tài)分布N（m，σ2C）在D 維的S 空間中采樣得到后代λ，m是當前概率分布的估計值的平均值，σ是搜索步長，C是高斯分布的協(xié)方差矩陣。然后再對樣本進行適應(yīng)度函數(shù)f(x)進行運算，選擇出適應(yīng)度最好的部分樣本更新參數(shù)m，C，σ生成下一代。多元高斯分布涉及隨機變量的任何優(yōu)先序列x1，x2，…xk，該分布通過最大似然法擬合，生成公式為

2.2 Tent混沌映射

針對CMA-ES 算法容易陷入局部最優(yōu)解的缺點，將Tent 混沌映射［8］的方法引入到CMA-ES 的算法中?；煦缡且环N普遍的非線性現(xiàn)象，具有隨機性，遍歷性和內(nèi)在規(guī)律性的特點。Tent混沌映射屬于線性映射，分布均勻且計算簡單，搜索效率較高。當1 ＜u＜2 時，其迭代表達式如下：

混沌序列的第n維服從均勻分布的隨機數(shù)是xn，n∈[1，N]，xn∈[0，1]。x的初值和u的初值不能相當，否則迭代更新方程將出現(xiàn)周期狀態(tài)而不是混沌狀態(tài)。圖1 為x初值為0.8，u為1.5 時，對初值進行500次迭代映射的分布圖。

圖1 Tent混沌映射分布圖

將混沌變量映射到解空間：

其中Ld和Ud分別為第n維變量對應(yīng)搜索的上下限，原均值第n維參數(shù)加上Δmd即可實現(xiàn)對均值的混沌映射。

2.3 萊維飛行隨機數(shù)

萊維飛行［9］指的是步長的概率分布為重尾分布的隨機行走，即在隨機行走的過程中有相對較高的概率出現(xiàn)大跨步。萊維飛行服從萊維分布的隨機搜索路徑，通過短距離行走和偶爾長距離行走的結(jié)合，達到更廣的隨機范圍。萊維飛行能夠擴大搜索范圍、增加種群多樣性，相比于其他隨機算法，更容易跳出局部最優(yōu)解，更快地達到全局最優(yōu)解。

一般通過Mantegna 算法來模擬萊維飛行。其更新公式為

表示xi第t代的位置，⊕表示點對點乘法，α表示步長控制量，Levy(λ)表示隨機搜索路徑，且滿足Levy～u=t-（λ1 ＜λ≤3）。

步長s計算公式為

其中u、v都為正態(tài)分布，β為定常數(shù)通常取1.5。

圖2 為Matlab 中萊維飛行進行1000 步的模擬圖，可以看出在不確定環(huán)境下萊維飛行具有更廣泛的搜索范圍和更快的全局搜索尋優(yōu)能力。

圖2 萊維飛行模擬圖

3 基于改進CMA-ES算法的踢球算法

基于改進CMA-ES算法具體步驟如下。

步驟一：機器人接收到踢球指令，通過踢球代價函數(shù)選擇最佳踢球點；

步驟二：利用貝塞爾曲線優(yōu)化最佳踢球點的行走路徑，并用三次樣條插值法確定踢球時腿部的軌跡曲線；

步驟三：在傳統(tǒng)的CMA-ES算法中加入Tent混沌映射和萊維飛行隨機數(shù)優(yōu)化迭代過程，滿足停止條件后跳出；

步驟四：基于優(yōu)化的參數(shù)，通過逆運動學(xué)反向求解各個關(guān)節(jié)角度；

步驟五：通過ZMP 判據(jù)判定該參數(shù)是否穩(wěn)定，如果穩(wěn)定，執(zhí)行踢球動作，如果不穩(wěn)定，轉(zhuǎn)步驟三。

算法流程圖如圖3所示。

圖3 基于改進CMA-ES的算法流程

3.1 最佳踢球點選擇

踢球點的選擇需要考慮機器人與球的相對位置，包括機器人與球之間的距離和矢量角，以及球到目標之間的矢量。以比賽環(huán)境為例，機器人和球的相對位置如圖4所示。

圖4 踢球點選擇示意圖

為了防止機器人在行走的過程中誤碰到球產(chǎn)生不必要的耗時和誤差，本文選擇將球以偏移量R為半徑進行虛擬膨脹，引入代價函數(shù)CKick對虛擬圓上所有的可行踢球點（K1，K2…Kn）計算其踢球代價，當CKick取最小值時即為最佳踢球點。

最佳踢球點選擇分為角度旋轉(zhuǎn)和路徑規(guī)劃，所以踢球代價函數(shù)CKick為旋轉(zhuǎn)代價分量Cturn和距離代價分量Cdist之和。

3.1.1 旋轉(zhuǎn)代價分量Cturn

如圖4 所示，機器人的朝向與向量r→形成的夾角為α，行走向量d→與向量r→形成的夾角為β，轉(zhuǎn)身代價分量Cturn為

3.1.2 距離代價分量Cdist

機器人可以通過視覺參數(shù)獲取到球相對于機器人的坐標，根據(jù)圖4，Cdist大小等于，已知機器人與球心位置所構(gòu)成的矢量r→與偏移位置，可算出距離代價分量Cdist：

根據(jù)比賽情況設(shè)定權(quán)重值ωd、ωt，進而可得CKick：

3.2 貝塞爾曲線優(yōu)化軌跡

3.2.1 最佳踢球點行走軌跡優(yōu)化

在傳統(tǒng)方法中，機器人會先補償旋轉(zhuǎn)代價分量Cturn然后沿→方向徑直走向K2踢球點，最后調(diào)整對齊至K1最佳踢球點。

為了解決傳統(tǒng)方法中行走效率低、拐點不平滑的缺陷，本文采用貝塞爾曲線進行路徑規(guī)劃，使機器人到達最佳踢球點時自身的朝向就是踢球的方向。設(shè)機器人初始點為P1，最佳踢球點為P′，中間點P2在機器人初始朝向的延長線上，中間點P3在最佳踢球點和球門中心連線的延長線上。用式（13）的三階貝塞爾曲線［10～11］生成的路徑軌跡如圖5所示。

圖5 基于貝塞爾曲線的路徑軌跡規(guī)劃

3.2.2 踢球腿部軌跡優(yōu)化

由于足球機器人的軀干節(jié)距旋轉(zhuǎn)量基本在3.0°范圍內(nèi)，所以本文假設(shè)機器人在踢球運動的過程中上半身保持直立。踢球運動時腿部動作主要分為抬腿、踢球和收腿三個階段。在執(zhí)行動作的過程中腿部各個關(guān)節(jié)的位姿時刻都在發(fā)生變化，根據(jù)其運動軌跡得到三條曲線，如圖6所示。

圖6 踢球腿部路徑圖

將曲線分為n個點代入式（13），通過三階貝塞爾曲線保證運動的曲線函數(shù)是光滑的，以實現(xiàn)對腿部軌跡的優(yōu)化。

3.3 逆運動學(xué)模塊

機器人運動學(xué)是通過計算默認位置和各個部位之間的連接以及各關(guān)節(jié)的角度來確定位置。本文通過逆運動學(xué)設(shè)定機器人的端點來計算必要的關(guān)節(jié)旋轉(zhuǎn)值以規(guī)劃動作。即軌跡規(guī)劃模塊給出腿部軌跡曲線，輸入到逆運動學(xué)模塊算出下一時刻的關(guān)節(jié)值［12～13］。

輸入值是齊次變換矩陣，包含了局部坐標系下的足部位置和方向。

相對于HipRot（臀部滾動關(guān)節(jié)）坐標系下的足部位姿：

式中l(wèi)d表示站立狀態(tài)下雙腿的間距，Rotx(v)表示向量v沿著x軸的旋轉(zhuǎn)值，Transy(v)表示向量v沿著y軸的平移值。

假設(shè)機器人大腿（lthigh）和小腿（llowerleg）之間形成一個三角形，并且已知的平移矢量，可以計算出膝關(guān)節(jié)角和踝關(guān)節(jié)角。

式中（x，y，z）是平移矢量的分量，atan2（y，x）定義x軸與點（x，y）之間的夾角。而髖關(guān)節(jié)的俯仰角、滾動角和偏轉(zhuǎn)角是由唯一決定的：

其中是可用式（13）、（14）確定的。

4 實驗分析

4.1 腿部軌跡驗證實驗

使用SimRobot 平臺［14］進行踢球仿真實驗并在實體機器人上進行驗證，結(jié)果如圖7 所示。圖7 代表腿部仿真軌跡曲線，其中白色點的集合代表機器人關(guān)節(jié)所在的坐標系，灰色的曲線代表各關(guān)節(jié)在當前狀態(tài)的運動軌跡，從左到右三組圖分別對應(yīng)踢球過程中最重要的三種狀態(tài)：圖7（a）是t=0.20s 時機器人向抬腿的瞬時位姿；圖7（b）是t=0.31s 時踢球的瞬時位姿；圖7（c）是t=0.45s 時機器人收腿復(fù)位的瞬時位姿。由圖7 仿真結(jié)果可以得出，三種狀態(tài)下仿真機器人不僅穩(wěn)定且腿部軌跡平滑容易實現(xiàn)。

圖7 踢球效果圖

由ZMP 穩(wěn)定判據(jù)［15～16］以及實體機器人的表現(xiàn)可知該方法穩(wěn)定且可行。具體踢球效果將在實驗4.2節(jié)進行分析。

4.2 踢球?qū)Ρ葘嶒?/h3>

將本文中改進的CMA-ES踢球算法與文獻［6］中的踢球方法進行了實驗比較，結(jié)果如組圖8 所示。上半部分為使用了改進CMA-ES 踢球算法的效果，而下半部分為文獻中［6］的踢球方法的效果。

圖8 踢球效果對比圖

由實驗可知，在任意時刻，上半圖球的滾動距離都要大于下半圖。而從最終球停止位置來看，改進CMA-ES 的踢球算法可以實現(xiàn)一次性把球從己方禁區(qū)踢至對方中場，最大距離可達6.9m，而文獻［6］的踢球?qū)嶒炞畲缶嚯x為3.8m。分別使用本文方法和文獻［6］的踢球方法，進行了100 次踢球?qū)嶒灉y試，分別從踢球的距離、執(zhí)行踢球動作所花的時間和踢球的穩(wěn)定性三個方面進行對比，實驗結(jié)果如表1 所示。得出本文方法的踢球距離平均為6.53m，文獻［6］的踢球距離平均為3.76m。執(zhí)行動作時間上，本文算法平均在2.80s，文獻［6］平均在4.26s。從以上對比可以看出，本文方法在踢球距離和時間上均優(yōu)于文獻［6］的方法，但穩(wěn)定性上略有不足。

表1 踢球?qū)嶒瀸Ρ?/p>

5 結(jié)語

本文主要介紹了標準平臺組足球比賽中基于改進CMA-ES的踢球算法，該方法作為比賽的關(guān)鍵技術(shù)之一，對足球機器人的進攻與防守效率有較大的提升，并在實際比賽中表現(xiàn)良好，在2020 年RoboCup 機器人世界杯中國公開賽標準平臺組項目中獲得了一等獎。

但同時，在實際的踢球過程中可能會有敵方機器人的干擾，該方法的不足之處在于動作的魯棒性有待提高。本文下一步工作是將強化學(xué)習(xí)理論應(yīng)用到踢球的動作優(yōu)化中，以進一步保證踢球過程中的魯棒性。