***(姓名)

***(學(xué)院)***@***.

第3章人形機器人行走控制基礎(chǔ)

《人形機器人技術(shù)基礎(chǔ)與應(yīng)用》3.1概述目錄3.2穩(wěn)定性分析3.3步態(tài)規(guī)劃3.4行走控制3.1概述人形機器人穩(wěn)定性分析人形機器人步態(tài)規(guī)劃人形機器人運動控制零力矩點（ZMP）穩(wěn)定性判據(jù)ZMP穩(wěn)定性分析完成各種任務(wù)的基礎(chǔ)基于ZMP穩(wěn)定性判據(jù)設(shè)計運動軌跡

對規(guī)劃軌跡或驅(qū)動力或力矩進行實時調(diào)節(jié)，使機器人能保持穩(wěn)定并完成期望的運動目標

模型預(yù)測控制3.2穩(wěn)定性分析人形機器人行走中的穩(wěn)定性一般是指機器人運動過程中在遇到擾動后回到原狀態(tài)的能力，包括如機械結(jié)構(gòu)變化等的內(nèi)部擾動和外部環(huán)境的擾動。根據(jù)機器人的機械結(jié)構(gòu)限制與控制方法，機器人能克服的擾動有一定的限度，當擾動過大時，機器人便會跌倒。3.2穩(wěn)定性分析穩(wěn)定性判據(jù)是判斷機器人在擾動下是否能夠保持穩(wěn)定行走的依據(jù)，是衡量穩(wěn)定性的重要依據(jù)，也是人形機器人步態(tài)規(guī)劃和行走控制設(shè)計的準則。你知道哪些穩(wěn)定性判據(jù)呢？3.2穩(wěn)定性分析3.2.1ZMP穩(wěn)定性判據(jù)零力矩點（ZMP）是一個理論上的點，在這個點上，機器人與地面接觸的垂直力產(chǎn)生的力矩與地面相關(guān)的所有外力和力矩之和為0。這個點通常位于機器人足部的底部投影內(nèi)。

ZMP的定義ZMP的數(shù)學(xué)表達及計算根據(jù)達朗貝爾原理，在參考坐標系中，得到人形機器人此時的受力平衡方程和對作用點ZMP的力矩平衡方程，即

3.2.1ZMP穩(wěn)定性判據(jù)

將式（3-2）和式（3-4）代入式（3-3），可得

考慮到??在水平方向的分量為0，式（3-5）在水平方向上的表達式為

下標?代表水平分量。坐標系??的方向為：??軸水平向右，??軸水平向前，??軸豎直向上，式（3-6）給出了ZMP的數(shù)學(xué)表達，并且提供了在水平地面上計算ZMP坐標的公式。將式（3-6）變形可得其標量計算公式為3.2.1ZMP穩(wěn)定性判據(jù)(3-7)(3-8)

3.2.1ZMP穩(wěn)定性判據(jù)在具備以下條件的情況下可以計算得到ZMP的位置人形機器人足部安裝的力或力矩傳感器能檢測到支撐反作用力的大小、方向和作用位置已知人形機器人身體各部分的轉(zhuǎn)動慣量、加速度及角加速度（機器人處于動態(tài)平衡時）人形機器人的腳踝裝有六維力或力矩傳感器第一種情況可以直接通過檢測的反作用力的信息計算出豎直方向等效作用力的作用點，即ZMP的位置；第二、三種情況可以應(yīng)用式（3-7）來計算ZMP的位置。3.2.1ZMP穩(wěn)定性判據(jù)3.2.1ZMP穩(wěn)定性判據(jù)ZMP穩(wěn)定性判據(jù)的應(yīng)用穩(wěn)定區(qū)域（StableRegion）是支撐腿的足部與地面接觸形成的凸多邊形支撐區(qū)域。穩(wěn)定性裕度（StabilityMargin）是仿人機器人行走穩(wěn)定程度的一種量化描述，其量化參數(shù)是ZMP與穩(wěn)定區(qū)域邊界的最短距離。穩(wěn)定區(qū)域穩(wěn)定裕度腳腳ZMP穩(wěn)定區(qū)域和穩(wěn)定裕度

ZMP越靠近穩(wěn)定區(qū)域的中心，與穩(wěn)定區(qū)域邊界的最短距離就越大，即穩(wěn)定性裕度越大，仿人機器人此時的姿態(tài)也就越穩(wěn)定。

穩(wěn)定區(qū)域最穩(wěn)定區(qū)域有效穩(wěn)定區(qū)域

3.2.1ZMP穩(wěn)定性判據(jù)有效穩(wěn)定區(qū)域是指穩(wěn)定區(qū)域內(nèi)的一個子區(qū)域，處于該子區(qū)域內(nèi)的所有ZMP對應(yīng)的穩(wěn)定性裕度大于外界環(huán)境干擾導(dǎo)致的該ZMP位置的變化量。有效穩(wěn)定區(qū)域可以用數(shù)學(xué)表達式描述，即

穩(wěn)定區(qū)域和有效穩(wěn)定區(qū)域

3.2.1ZMP穩(wěn)定性判據(jù)

(3-10)(3-11)當機器人可能遇到的最大擾動幅度已知時，就能得到對應(yīng)的有效穩(wěn)定區(qū)域。若ZMP落在有效穩(wěn)定區(qū)域內(nèi)，即使受到外界環(huán)境的干擾，也無須進行姿態(tài)穩(wěn)定性調(diào)整，人形機器人還能夠保持自身姿態(tài)穩(wěn)定并維持行走；當ZMP落在穩(wěn)定區(qū)域內(nèi)，但在有效穩(wěn)定區(qū)域外時，機器人在沒有外界干擾時仍是穩(wěn)定的，如果受到干擾就可能不穩(wěn)定，需要進行姿態(tài)穩(wěn)定性調(diào)整；當ZMP落在穩(wěn)定區(qū)域外時，機器人將變得不穩(wěn)定，隨時可能傾覆，需要立即對機器人姿態(tài)進行調(diào)整，使ZMP回到穩(wěn)定區(qū)域，甚至有效穩(wěn)定區(qū)域內(nèi)。3.2.1ZMP穩(wěn)定性判據(jù)ZMP穩(wěn)定性判據(jù)的注意事項和局限ZMP只能出現(xiàn)在支撐多邊形內(nèi)，不可能出現(xiàn)在支撐多邊形外ZMP與壓力中心（CoP）的聯(lián)系和區(qū)別

在動態(tài)平衡的情況下，CoP和ZMP的位置是一致的。當機器人處于非動態(tài)平衡狀態(tài)時，ZMP不存在，而CoP處于支撐多邊形的邊緣。ZMP穩(wěn)定性判據(jù)在以下幾種情況下將不再適用

1）機器人腳掌與地面的接觸面積很小

2）允許機器人支撐腿腳部相對地面轉(zhuǎn)動

3）需要判斷機器人腳底與地面是否有相對滑動時

3.2.2其他穩(wěn)定性判據(jù)李雅普諾夫穩(wěn)定性李雅普諾夫穩(wěn)定性是基于系統(tǒng)的狀態(tài)方程和李雅普諾夫函數(shù)來分析系統(tǒng)的穩(wěn)定性。如果存在一個李雅普諾夫函數(shù)，使得在系統(tǒng)的所有狀態(tài)下該函數(shù)都是非增的，那么系統(tǒng)被認為是穩(wěn)定的。1）正定性：李雅普諾夫函數(shù)需要在除平衡點以外的局部區(qū)域內(nèi)正定。2）能量耗散：函數(shù)隨時間的導(dǎo)數(shù)應(yīng)小于或等于零，表明系統(tǒng)不會隨時間增加能量。3.2.2其他穩(wěn)定性判據(jù)李雅普諾夫穩(wěn)定性計算方法1）確定系統(tǒng)的動力學(xué)方程：首先明確系統(tǒng)的狀態(tài)方程，描述系統(tǒng)如何從一個狀態(tài)過渡到另一個狀態(tài)。2）選擇或構(gòu)造李雅普諾夫函數(shù)：基于系統(tǒng)的物理特性和動力學(xué)行為設(shè)計李雅普諾夫函數(shù)。3）分析函數(shù)的導(dǎo)數(shù)：計算李雅普諾夫函數(shù)相對于時間的導(dǎo)數(shù)，并證明其在所有相關(guān)狀態(tài)下非正。3.2.2其他穩(wěn)定性判據(jù)基于落腳點的穩(wěn)定性判據(jù)落腳點（FootPlacementEstimator，F(xiàn)PE）是一種評價穩(wěn)定性的方法，該方法計算擺動腿在什么位置著地能夠獲得穩(wěn)定步態(tài)。基本前提假設(shè)：系統(tǒng)的角動量在擺動腿與地面碰撞前后是不變的，基本思想是讓機器人擺動腿與地面碰撞后，系統(tǒng)的機械能等于其最大勢能。3.2.2其他穩(wěn)定性判據(jù)計算方法根據(jù)系統(tǒng)在碰撞前后相對于擺動腿觸地點的角動量守恒，有

(3-12)

(3-13)

3.2.2其他穩(wěn)定性判據(jù)計算方法

(3-14)

(3-15)根據(jù)幾何關(guān)系，可以得到擺動腿落地點在地面上相對于質(zhì)心的位置為(3-16)當擺動腿落地點在FPE前方，系統(tǒng)可以在不需要主動減速的情況下，在接下來的一步內(nèi)達到靜止站立。如果擺動腿落地點在FPE后方，則需要多步才能達到靜止站立。

3.3步態(tài)規(guī)劃3.3.1倒立擺模型模型原理倒立擺模型包括一個質(zhì)點（倒立擺的質(zhì)心）和一個無質(zhì)量的桿（連接質(zhì)點和旋轉(zhuǎn)軸），桿固定在一個可移動的基座上。當施加力或扭矩時，質(zhì)點的運動軌跡需要通過控制策略來穩(wěn)定。數(shù)學(xué)描述倒立擺的運動可以通過牛頓第二定律來描述，即(3-17)

3.3.2ZMP步態(tài)規(guī)劃ZMP步態(tài)規(guī)劃概述在人形機器人的步態(tài)規(guī)劃中，ZMP理論是核心技術(shù)之一，它幫助確保機器人在行走和執(zhí)行任務(wù)時的動態(tài)穩(wěn)定性。根據(jù)期望的ZMP軌跡生成機器人行走步態(tài)的方法的基本思路是：根據(jù)機器人期望的步長、周期等信息確定機器人期望的ZMP軌跡根據(jù)期望的ZMP軌跡得到機器人質(zhì)心的運動軌跡得到機器人各個關(guān)節(jié)的軌跡3.3.2ZMP步態(tài)規(guī)劃ZMP步態(tài)規(guī)劃概述預(yù)觀控制方法是Sheridan等在20世紀60年代提出的一種控制器設(shè)計方法，最早應(yīng)用在線性二次最優(yōu)伺服控制器的設(shè)計上，后來通過離散化，應(yīng)用到人形機器人的步態(tài)規(guī)劃中。ZMP步態(tài)規(guī)劃是指利用零力矩點理論來設(shè)計機器人的行走步態(tài)，確保其在移動過程中的穩(wěn)定性。通過控制ZMP軌跡使其始終位于支撐腳的底部投影內(nèi)，可以有效避免機器人在行走過程中的搖擺和倒下。3.3.2ZMP步態(tài)規(guī)劃ZMP軌跡的生成方法

ZMP軌跡反映了機器人與地面交互的動態(tài)穩(wěn)定點的路徑理想的ZMP軌跡應(yīng)當始終位于機器人的支撐面內(nèi)，通常是兩腳之間或單腳的底部區(qū)域軌跡的生成依賴于機器人的運動學(xué)參數(shù)和動力學(xué)響應(yīng)ZMP軌跡的生成是步態(tài)規(guī)劃中的關(guān)鍵步驟，它涉及以下幾個方面：參考軌跡設(shè)計：理想的ZMP軌跡，通常是一條平滑的曲線，確保它穿過或接近支撐腳的幾何中心。軌跡優(yōu)化：通過優(yōu)化算法調(diào)整ZMP軌跡，減少峰值扭矩和力的需求，提高能量效率。3.3.2ZMP步態(tài)規(guī)劃預(yù)觀控制法進行人形機器人步態(tài)規(guī)劃的主要思想是根據(jù)將來的ZMP參考軌跡得到當前的質(zhì)心軌跡，使得到的質(zhì)心軌跡對應(yīng)的ZMP軌跡能較好地跟蹤期望的ZMP軌跡，從而使ZMP始終處于支撐范圍內(nèi)，得到穩(wěn)定的運動。

三維空間中的倒立擺模型

系統(tǒng)的動力學(xué)方程為(3-18)(3-19)3.3.2ZMP步態(tài)規(guī)劃

(3-20)(3-21)

(3-22)(3-23)3.3.2ZMP步態(tài)規(guī)劃式（3-22）、式（3-23）可以寫為(3-24)(3-25)式（3-24）、式（3-25）給出了通過系統(tǒng)的質(zhì)心軌跡計算ZMP位置的表達式。如果將機器人表達為類似的倒立擺模型，則在得到機器人質(zhì)心的運動軌跡之后，就可以利用這個方程計算其ZMP的位置。3.3.2ZMP步態(tài)規(guī)劃質(zhì)心軌跡的計算方法

(3-26)(3-27)則式（3-23）、式（3-24）可以寫成以下形式：(3-28)(3-29)3.3.2ZMP步態(tài)規(guī)劃(3-30)(3-31)式（3-28）~（3-31）描述的系統(tǒng)以質(zhì)心的加速度的導(dǎo)數(shù)作為輸入，以ZMP的位置作為輸出，可以作為跟蹤參考ZMP軌跡的步態(tài)生成器。基本思想在機器人的行走過程中，當支撐腿變化時，ZMP的位置往往會有一個階躍變化，而正常情況下，質(zhì)心的位置需要在ZMP階躍變化之前就開始逐漸變化。也就是說，在計算質(zhì)心軌跡時，需要提前考慮到未來ZMP的變化趨勢。根據(jù)將來的ZMP的變化趨勢計算當前質(zhì)心的軌跡。

3.3.2ZMP步態(tài)規(guī)劃控制器ZMP方程ZMP

參考軌跡Pref+-PuZMPx,yCoM通過ZMP跟蹤控制實現(xiàn)步態(tài)生成的流程圖

將式（3-30）和式（3-31）進行離散化，假定采樣時間為??，則得到??方向的系統(tǒng)方程為(3-32)(3-33)

3.3.2ZMP步態(tài)規(guī)劃按照預(yù)觀控制理論，衡量跟蹤指標的表達式為(3-34)

(3-35)

3.3.2ZMP步態(tài)規(guī)劃

3.3.3其他步態(tài)規(guī)劃基于穩(wěn)定性裕度的步態(tài)規(guī)劃方法

基本思想：先根據(jù)地面環(huán)境設(shè)定足部軌跡，在可變參數(shù)的有效范圍內(nèi)找出具有最大穩(wěn)定性裕度的軀干軌跡作為最后的規(guī)劃結(jié)果。

人形機器人踝關(guān)節(jié)和髖關(guān)節(jié)的運動

為了使機器人適應(yīng)不同的地面條件，首先要明確兩只腳的運動軌跡，尤其是踝關(guān)節(jié)的運動軌跡，然后再確定髖關(guān)節(jié)的運動軌跡。

3.3.3其他步態(tài)規(guī)劃三次樣條插值

(3-36)

(3-37)3.3.3其他步態(tài)規(guī)劃三次樣條插值

(3-38)

(3-39)

(3-40)

3.3.3其他步態(tài)規(guī)劃三次樣條插值

(3-41)

(3-42)3.3.3其他步態(tài)規(guī)劃三次樣條插值

(3-43)式中(3-44)3.3.3其他步態(tài)規(guī)劃三次樣條插值

(3-45)3.3.3其他步態(tài)規(guī)劃足部軌跡規(guī)劃仿人型機器人的足部軌跡是根據(jù)地面環(huán)境來設(shè)定的，例如地面凹凸不平或者有障礙物等情況下，應(yīng)該根據(jù)地面約束條件來設(shè)定足部軌跡。

人形機器人一步運動的示意圖

3.3.3其他步態(tài)規(guī)劃足部軌跡規(guī)劃

(3-46)由此可以得到右腳踝關(guān)節(jié)的約束條件為(3-47)3.3.3其他步態(tài)規(guī)劃足部軌跡規(guī)劃(3-48)

3.3.3其他步態(tài)規(guī)劃足部軌跡規(guī)劃

(3-49)(3-50)(3-51)

3.3.3其他步態(tài)規(guī)劃足部軌跡規(guī)劃

機器人上臺階時的運動參數(shù)

3.3.3其他步態(tài)規(guī)劃足部軌跡規(guī)劃人形機器人上臺階時，腳部位置應(yīng)該滿足下面的約束條件，即(3-52)(3-53)

3.3.3其他步態(tài)規(guī)劃腰部軌跡規(guī)劃

(3-54)同樣，可以由三次樣條插值方法，得到滿足式（3-53）并且二階導(dǎo)數(shù)連續(xù)的軌跡。3.3.3其他步態(tài)規(guī)劃腰部軌跡規(guī)劃

一個行走周期可以分為三個階段：①起始階段，人形機器人的運動速度由0加速到期望的速度；②穩(wěn)定行走階段，人形機器人以期望的速度行走；③結(jié)束階段，人形機器人從期望速度減速行走至速度為0。3.3.3其他步態(tài)規(guī)劃腰部軌跡規(guī)劃為了得到周期性的平滑軀干軌跡，還必須滿足約束條件，即(3-56)

(3-55)3.3.3其他步態(tài)規(guī)劃腰部軌跡規(guī)劃(3-57)3.3.3其他步態(tài)規(guī)劃腰部軌跡規(guī)劃

(3-58)

(3-59)

3.3.3其他步態(tài)規(guī)劃腰部軌跡規(guī)劃另外，根據(jù)起始時刻及結(jié)束時刻機器人的速度為0，可以得到以下邊界條件(3-60)

根據(jù)前面提到的方法，可以得到步態(tài)生成方法的流程：根據(jù)地面環(huán)境規(guī)劃出滿足地面約束條件的腳部軌跡；根據(jù)ZMP穩(wěn)定性判據(jù)準則，規(guī)劃出具有高穩(wěn)定性的腰部軌跡。腳部及腰部軌跡確定之后，通過逆運動學(xué)計算得到各關(guān)節(jié)的運動軌跡。

3.3.3其他步態(tài)規(guī)劃基于機器學(xué)習(xí)的步態(tài)規(guī)劃

機器學(xué)習(xí)在步態(tài)規(guī)劃中的應(yīng)用

機器學(xué)習(xí)方法可以自動從大量數(shù)據(jù)中提取模式，用于預(yù)測和優(yōu)化步態(tài)。這些方法包括但不限于以下三種：1）監(jiān)督學(xué)習(xí)。使用帶標簽的數(shù)據(jù)來訓(xùn)練模型2）強化學(xué)習(xí)。通過與環(huán)境的交互試錯來發(fā)現(xiàn)優(yōu)化策略3）無監(jiān)督學(xué)習(xí)。從未標記的數(shù)據(jù)中學(xué)習(xí)步態(tài)模式技術(shù)實現(xiàn)

1）先收集大量的機器人運動數(shù)據(jù)2）從原始數(shù)據(jù)中提取有用的特征3）使用機器學(xué)習(xí)算法來訓(xùn)練步態(tài)生成模型4）最后進行模型評估與優(yōu)化，并根據(jù)實驗結(jié)果調(diào)整和優(yōu)化模型。確保模型的泛化能力和處理大量數(shù)據(jù)的能力是實現(xiàn)這一目標的關(guān)鍵挑戰(zhàn)。3.4行走控制模型預(yù)測控制簡介

模型預(yù)測控制

3.4.1模型預(yù)測控制人形機器人的理想單剛體模型是將整個機器人假設(shè)為一個剛體，通過兩個足端與環(huán)境交互，它通過受到的外力和外力矩作用來改變自身運動狀態(tài)。

單剛體模型

3.4.1模型預(yù)測控制單剛體模型

根據(jù)剛體的質(zhì)心動力學(xué)，單剛體模型的數(shù)學(xué)形式有

（3-61）

（3-62）

（3-63）

3.4.1模型預(yù)測控制

（3-64）根據(jù)式（3-63）和式（3-64）可以得到世界坐標系下機身角速度與歐拉角導(dǎo)數(shù)的關(guān)系為

（3-65）3.4.1模型預(yù)測控制

（3-66）或者寫為

（3-67）

（3-68）3.4.1模型預(yù)測控制

（3-69）

（3-70）

（3-71）3.4.1模型預(yù)測控制

（3-72）

（3-73）將式（3-73）經(jīng)過前向歐拉法進行離散，得到離散時間線性狀態(tài)方程為

（3-74）

（3-75）

（3-76）3.4.1模型預(yù)測控制3.4.1模型預(yù)測控制人形機器人模型預(yù)測控制問題由代價函數(shù)、預(yù)測模型和約束三部分組成

（3-77）

模型預(yù)測控制問題構(gòu)造與求解3.4.1模型預(yù)測控制人形機器人運動控制任務(wù)中，等式約束主要是根據(jù)步態(tài)規(guī)劃，將擺動腿的足端力和力矩置為0。不等式約束針對支撐腿，包括法向力、力矩最小最大值約束，避免較大的足端力和力矩導(dǎo)致關(guān)節(jié)力矩超限；用足端摩擦錐約束，避免打滑。

（3-78）

（3-79）

3.4.1模型預(yù)測控制

（3-80）

（3-81）3.4.1模型預(yù)測控制式（3-81）中，第三項為常數(shù)，因此代價函數(shù)主要受前兩項影響。二次規(guī)劃問題的標準數(shù)學(xué)形式為

（3-82）

將模型預(yù)測控制問題轉(zhuǎn)化為標準形式可得

（3-83）

（3-84）至此，完成了對人形機器人模型預(yù)測控制問題進行構(gòu)造轉(zhuǎn)化為二次規(guī)劃問題的標準形式。實際求解時可以借助OSQP、OOQP等開源的求解器進行求解。

全身控制的基本原理

機器人全身控制（WholeBodyControl,WBC）通過控制機器人的所有可驅(qū)動關(guān)節(jié)，來優(yōu)化機器人的整體行為。

3.4.2全身控制

（3-85）3.4.2全身控制

平面三自由度機械臂的簡化示意圖

3.4.2全身控制可以引入機器人全身控制的思想，對控制任務(wù)進行分層，這里將末端位置控制任務(wù)作為第一優(yōu)先級，關(guān)節(jié)角控制任務(wù)作為第二優(yōu)先級。在優(yōu)先完成高優(yōu)先級控制任務(wù)的前提條件下，盡可能完成低優(yōu)先級控制任務(wù)。

為了描述這種冗余特性，定J(N)為雅可比矩陣J的零空間投影矩陣，簡稱為零空間矩陣，J(N)與J滿足特性

J(N)=0（3.86）

（3-87）3.4.2全身控制

（