一種四足機器人優化足底力分配和軌跡跟蹤的方法

2024-04-15 19:48:05 1

1.本發明涉及一種用於四足機器人足底力的分配和軌跡跟蹤方法，該方法基於虛擬模型控制進行優化，屬於四足機器人的控制技術領域。


背景技術：

2.四足機器人是近年來發展較快的一種腿足式機器人，與履帶式或輪式機器人相比，在面對複雜環境條件時有著巨大的優勢，在近幾年得到了廣泛的發展。四足機器人常見的步態有trot、bounding和gallop等步態，足底力分配和軌跡跟蹤性能直接影響到四足機器人的穩定。
3.虛擬模型控制的原理是將四足機器人的每條腿虛擬為彈簧和阻尼器，通過分析機器人足端所受的虛擬力通過雅可比矩陣建立虛擬力和關節力的映射關係。在控制上只需要調整虛擬元件對應的參數就可以實現控制對象預期的運動特性。虛擬模型控制可以將四足機器人本體控制簡化為對虛擬剛體高度、速度和姿態的獨立控制，由於虛擬模型控制不用考慮動力學複雜的計算關係即可實現良好的力控特性，通過這種方法可以實現四足機器人的多種穩定步態。四足機器人可通過二次規劃來求解最優足底力，但是無法應對存在多個任務需求的情況。在分層二次優化(hqp)中，多任務控制問題被重新定義為分層優化問題，分層目標的每個層次都被建模為二次規劃，hqp能夠通過向較低優先級目標添加額外的線性約束來解決等式和不等式的層次結構，使低優先級目標的解屬於高優先級空間的解空間來實現目標側重。
4.基於虛擬模型控制的四足機器人trot步態分為支撐相控制和搖擺相控制。對於目前的四足機器人虛擬模型控制技術，存在以下問題：
5.在支撐相期間僅通過傳統的虛擬模型控制算法求解軀幹虛擬力，使得機器人姿態角和支撐腿的地面反作用力的波動範圍較大，使四足機器人喪失了一定的行走穩定性和魯棒性；在搖擺相期間受虛擬彈簧模型的影響，軌跡跟蹤誤差較大，進而影響相位切換時的落足點控制，使四足機器人無法達到預期運動速度。


技術實現要素：

6.本發明針對現有四足機器人行走過程中的足力分配和軌跡跟蹤技術存在的不足，基於虛擬模型控制，提出一種優化的四足機器人的足底力分配和軌跡跟蹤方法，使四足機器人行進過程能夠保持良好的動態穩定性。
7.本發明的四足機器人優化足底力分配和軌跡跟蹤的方法，採用以下方案：
8.(1)在四足機器人支撐相期間，將機器人支撐相分為支撐相前半段和支撐相後半段，應用分層二次規劃(hqp)，結合虛擬模型控制算法，將支撐相的姿態穩定和質心軌跡跟蹤兩個主要任務都轉換為二次規劃問題並求解，通過零空間投影得到最終優化足底力，並在支撐相分段切換瞬間，實現不同任務之間優先級對調，在支撐相前半段著重調節軀幹虛擬扭矩，實現機器人著地後的機體穩定性能，在支撐相後半段著重調節軀幹虛擬力，實現機
器人離地前的質心軌跡跟蹤。
9.首先給定四足機器人支撐相的質心軌跡和預期rpy角，通過虛擬模型控制算法求解軀幹虛擬力和虛擬扭矩，並將軀幹虛擬力、虛擬扭矩和足端給定力之間的力分配問題轉化成分層二次規劃(hqp)問題，對機器人機體姿態穩定和機體質心軌跡跟蹤兩個支撐相主要任務轉換為兩個qp問題，通過零空間投影法對最優解進行處理，並在支撐相分段切換的瞬間實現不同任務之間優先級對調，以此實現不同階段調節量之間的側重，同時防止過大的足端虛擬力；利用qpoases對轉換成的兩個二次優化問題進行求解，得到最優足端虛擬力，並通過雅可比矩陣映射到各關節扭矩。
10.所述支撐相前半段分層二次規劃(hqp)的最優解為：
[0011][0012]
所述支撐相後半段分層二次規劃(hqp)的最優解為：
[0013][0014]
其中是關於軌跡跟蹤qp問題的解，是關於姿態穩定qp問題的解，表示矩陣ai的零空間投影矩陣；使用矩陣的偽逆來構成零空間投影矩陣，即其中ii是單位矩陣，是矩陣ai的偽逆。
[0015]
(2)在四足機器人搖擺相期間，對預先得到的擺動相預期軌跡進行二次求導，得到擺動相各點加速度關於時間t的表達式，建立擺動相虛擬彈簧剛度與擺動相各點加速度之間的數值關係，實現四足機器人擺動相的虛擬剛度加速度適應控制，結合擺動相的關節角度比例微分控制，減小擺動相軌跡跟蹤誤差。
[0016]
具體是在四足機器人搖擺相期間，首先根據機體各方向預期速度，引入實際速度與目標速度之間誤差的比例控制，計算足端與地面各方向零度角，並依據各方向零度角通過插值多項式得到平滑的擺動相預期軌跡，實現擺動相期間足端的平滑運動；然後求解擺動相預期軌跡的二次導函數，得到擺動相各點加速度關於時間t的表達式，將某時刻加速度與後一時刻加速度之比定義為當前時刻剛度比β，建立擺動相虛擬彈簧剛度與擺動相各點加速度之間的數值關係，實現四足機器人擺動相的虛擬剛度加速度適應控制；在此基礎上，根據位置控制的思想通過逆運動學解算各關節的預期角度，並與各關節實際角度相比較，實現擺動相的關節角度比例微分控制。
[0017]
所述擺動相虛擬彈簧剛度與擺動相各點加速度之間的數值關係(擺動相某一時刻的虛擬彈簧剛度關於時間t的表達式)為：
[0018][0019][0020][0021]
stiffnessx(0)、stiffnessy(0)和stiffnessz(0)為擺動相各方向初始虛擬彈簧係數，數，和為擺動相各方向加速度關於時間的表達式。
[0022]
本發明通過控制算法的改進優化了四足機器人的足底力分配和軌跡跟蹤性能，同
時提升了整個控制過程的平滑度，使四足機器人行進過程能夠保持良好的動態穩定性。具有以下特點：
[0023]
1.應用啟發式方法分析四足機器人支撐相功能，應用分層二次規劃(hqp)，將支撐相的姿態穩定和質心軌跡跟蹤兩個主要任務都轉換為二次規劃問題並求解，通過零空間投影得到最終優化足底力，並在支撐相分段切換瞬間，實現不同任務之間優先級對調，在支撐相前半段著重實現機器人姿態穩定，支撐相後半段著重實現機器人質心軌跡跟蹤。
[0024]
2.求解擺動相預期軌跡關於時間t的二階分析模型，得到擺動相各點關於時間t的加速度表達式，將某時刻加速度與後一時刻加速度之比定義為當前時刻剛度比β，建立擺動相虛擬彈簧剛度與擺動相各點加速度之間的數值關係，實現四足機器人擺動相的虛擬剛度加速度適應控制，並結合關節角度比例微分控制優化擺動相的軌跡跟蹤性能。
附圖說明
[0025]
圖1是本發明四足機器人控制過程框圖。
[0026]
圖2是四足機器人軀幹的虛擬彈簧模型示意圖。
[0027]
圖3是分層二次規劃中應用零空間投影的優先級排序示意圖。
[0028]
圖4是四足機器人擺動足的虛擬彈簧模型示意圖。
具體實施方式
[0029]
四足機器人整個運動階段被分為支撐相和擺動相，這兩個階段由狀態機交替切換。本發明分別對四足機器人支撐相和搖擺相進行控制，控制架構如圖1所示。
[0030]
在支撐相期間，為了跟蹤軀幹的期望運動和姿態，將軀幹和支撐足看作一個虛擬彈簧模型，如圖2。由於腿部質量相較於軀幹質量很小，所以在控制過程中忽略腿部質量。對支撐足進行受力分析，軀幹的所受虛擬力和虛擬力矩由下式可得：
[0031][0032]
其中，fb和tb分別是機器人軀幹所受虛擬力和虛擬力矩，ff和fh分別是前支撐足和後支撐足足端受力，bpf和bph分別是前、後支撐足在軀幹坐標系下的位置。根據虛擬模型控制，軀幹所受虛擬力和虛擬力矩由下式可得：
[0033][0034][0035][0036][0037][0038][0039]
其中，f
x
、fy、fz、t
x
、ty和tz分別是應用虛擬模型控制後質心所受各方向虛擬力和虛擬力矩，stiffness和damping分別是所應用虛擬彈簧模型的虛擬彈簧係數和虛擬阻尼係數，xd、yd、zd、αd、βd和γd分別是機器人質心各方向期望位置和期望姿態角，x、y、z、α、β和γ
分別是質心各方向實時位置和實時姿態角，和分別是質心各方向期望速度和期望姿態角速度，和分別是質心各方向實際速度和實際姿態角速度。
[0040]
四足機器人切換成支撐相後需要實現機體穩定和質心軌跡跟蹤。在四足機器人支撐相期間，本發明將支撐相分為支撐相前半段和支撐相後半段，在不同分段著重實現不同的性能要求；在支撐相前半段著重調節軀幹虛擬扭矩，實現機器人著地後的機體穩定性能；在支撐相後半段著重調節軀幹虛擬力，實現機器人離地前的質心軌跡跟蹤。
[0041]
為了將軀幹所受虛擬力和虛擬力矩求解足底力分別轉換成兩個二次規劃(qp)問題，實現分層二次規劃(hqp)，結合上式可得：
[0042][0043][0044]
其中，[f
x f
y fz]
t
和[t
x t
y tz]
t
分別是qps問題中的b1、b2陣，和分別是qps問題中的a1和a2陣，[f
fx f
fy f
fz f
hx f
hy f
hz
]
t
是提出的分層二次規劃問題所求解x。
[0045]
為了簡化重力的影響，假設機器人軀幹的橫滾角和俯仰角都很小，則在垂直方向虛擬力上引入重力項。
[0046]
經上式定義，本發明將根據期望軀幹虛擬力和虛擬力矩求解最優足底力轉化成hqp問題，hqp使用零空間投影對最終優化解進行處理，使最終解更側重於完成其中一項任務，來實現支撐相不同階段軀幹虛擬力和虛擬力矩的優先級控制。可得：
[0047][0048]
其中，xi是所有qps的相同決策變量向量，代表前後支撐足各方向受力。下標i表示任務的優先級。q∈r3×3是權重對角矩陣，用來控制足底力對軀幹虛擬力和虛擬力矩的影響大小。w∈r6×6是懲罰對角矩陣，可以防止過大的足底力。為了防止腳步滑動，增加了摩擦錐約束，其中μ是摩擦係數，同時添加垂直力大於o的約束。
[0049]
本發明在四足機器人支撐相由於僅有兩個任務優先級，並且在支撐相前半段和後半段切換瞬間，對姿態穩定和跟蹤質心軌跡兩個任務進行優先級對調。所以在支撐相前半段，為了減小機器人機體姿態的變化，使機器人在著地後機體姿態迅速達到期望狀態，故實現機器人姿態穩定性能具有更高的優先級，則支撐相前半段hqp的最優解為：
[0050][0051]
在支撐相後半段，為了減小機器人質心軌跡的跟蹤誤差，使機器人在離地前到達期望位置並具備離地的加速度，故實現機器人質心軌跡跟蹤具有更高的優先級，則支撐相後半段的最優解為：
[0052][0053]
其中是關於軌跡跟蹤qp問題的解，是關於姿態穩定qp問題的解，表示矩陣ai的零空間投影矩陣。本發明使用矩陣的偽逆來構成零空間投影矩陣，即ni＝i
i-其中ii是單位矩陣，是矩陣ai的偽逆。應用零空間投影對解進行處理的過程如圖3。根據τ＝-j
t
x
*
，將最終求得的最優支撐足力通過雅可比矩陣可求得支撐足各關節的最佳輸入扭矩。
[0054]
四足機器人搖擺相的主要任務是通過控制虛擬彈簧落地時與地面的角度來控制機體的速度，所以對於搖擺相來說足端軌跡的精確跟蹤顯得尤為重要。在四足機器人搖擺相期間，由於虛擬彈簧的滯後特性，跟蹤足端軌跡的效果較差。
[0055]
在搖擺相期間，將擺動足看作一個虛擬模型，如圖4。首先通過落足點控制來控制機器人各方向速度，同時引入實際速度與目標速度之間誤差的比例控制，得到落足點的實際坐標。為了得到擺動相的平滑軌跡曲線，對擺動相足端軌跡施加狀態約束，可得擺動相各方向平滑的多項式插值足端軌跡。
[0056]
根據虛擬模型控制，擺動腿的虛擬力由下可得：
[0057][0058][0059][0060]
其中，f
x
、fy和fz分別是應用虛擬模型控制後足端所受各方向虛擬力stiffness、damping分別是所應用虛擬彈簧模型的虛擬彈簧係數和虛擬阻尼係數，xd、yd和zd是分別是足端各方向期望位置，x、y和z分別是足端各方向實時位置，和分別是足端各方向期望速度，和分別是足端各方向實際速度。
[0061]
由於在擺動相期間，恆定的彈簧係數難以保證精確的擺動相軌跡跟蹤。為此本發明在四足機器人擺動相引入虛擬剛度加速度適應控制，實現擺動相足端虛擬彈簧係數隨足端各時刻加速度改變而改變。首先，由於虛擬模型中虛擬阻尼係數很小，且假設實現軌跡跟蹤後期望位置和實際位置的誤差很小，以x方向為例，即x
d-x＝ε，ε是一個極小的常數。則擺動腿各方向虛擬力可簡化為僅與虛擬彈簧係數有關的表達式：
[0062]fx
≈stiffness
x
*ε，
[0063]fy
≈stiffnessy*ε，
[0064]fz
≈stiffnessz*ε。
[0065]
求解擺動相各方向軌跡的二次導函數，可以得到擺動相各方向加速度關於時間的表達式和根據f＝ma可得：
[0066][0067][0068][0069]
其中m
*
為關於擺動足質量的參數，則擺動相各方向t時刻剛度為：
[0070][0071][0072][0073]
可知擺動相各方向虛擬彈簧係數正比於各方向加速度大小。令擺動相各方向t時刻剛度與(t+1)時刻剛度相比可得：
[0074][0075][0076][0077]
其中β
x
、βy和βz是定義的擺動相各時刻剛度比，給定擺動相初始虛擬彈簧係數為stiffness
x
(0)、stiffnessy(0)和stiffnessz(0)，則擺動相某一時刻的虛擬彈簧剛度關於時間t的表達式為：
[0078][0079][0080][0081]
由於擺動相各方向加速度在t≥0時有零值，因此虛擬彈簧剛度可能會出現為零或者過大的情況，故添加如下約束：
[0082]
0.1*stiffness
x
(0)≤stiffness
x
(t)≤10*stiffness
x
(0)，
[0083]
0.1*stiffnessy(0)≤stiffnessy(t)≤10*stiffnessy(0)，
[0084]
0.1*stiffnessz(0)≤stiffnessz(t)≤10*stiffnessz(0)。
[0085]
經過引入擺動相的虛擬剛度加速度適應控制，擺動相虛擬力變為：
[0086][0087][0088][0089]
得到擺動相各時刻足端虛擬力後，通過雅可比矩陣可求得擺動足各關節的輸入扭矩。此外，為了進一步精確軌跡跟蹤性能，根據位置控制的思想通過逆運動學解算擺動足每
一時刻各關節的預期角度，並與擺動足每一時刻各關節實際角度相比較，實現擺動相的關節角度比例微分控制。則擺動相各關節最終輸入扭矩為：
[0090][0091][0092][0093]
其中，k
θ
和d
θ
分別是關節角度比例微分控制的比例係數和微分係數，θ(t)和θd(t)分別是各關節實際角度和期望角度，和分別是各關節實際角速度和期望角速度。