0、步態(tài)規(guī)劃
四足機(jī)器人控制當(dāng)中，步態(tài)是至關(guān)重要的一項。我們可以簡單理解成四足機(jī)器人運(yùn)動過程中各腿的狀態(tài)，在這套設(shè)計方案中，我們對步態(tài)的規(guī)劃主要分成兩大主要部分，即接觸狀態(tài)和周期函數(shù)。而步態(tài)規(guī)劃的目的，就是創(chuàng)建一個關(guān)于的足端接觸狀態(tài)的周期函數(shù)。
1、接觸狀態(tài)
四足機(jī)器人行進(jìn)過程中，根據(jù)足端與地面是否發(fā)生接觸，我們可以規(guī)定各條腿的兩種接觸狀態(tài)，即接觸（contact）與擺動（swing）
總控制器會根據(jù)步態(tài)規(guī)劃給出的狀態(tài)，移交至對應(yīng)的控制器去處理，即擺動腿控制器（swing leg controll），接觸腿控制器（contact leg control）。
對于這兩種狀態(tài)，可以簡單地用一個布爾類型的值s來定義，即
對于周期性的步態(tài)規(guī)劃，我們可以用下標(biāo)Φ來進(jìn)行區(qū)分，可以寫成：
2、步態(tài)周期
四足機(jī)器人的運(yùn)動歸根到底都是周期運(yùn)動，我們無須量化機(jī)器人運(yùn)動的整個過程，因此如何量化定義一個完整的步態(tài)周期顯得極為重要，在此我們使用基于時間的周期函數(shù)，定義一個基準(zhǔn)相位值，公式如下：
其中，
t：當(dāng)前運(yùn)行時間
t0：當(dāng)前周期開始時間
T：一個步態(tài)周期
同樣我們可以利用取余的方法來簡化上述周期函數(shù)，兩者并無實質(zhì)的區(qū)別：
其中
%為取余運(yùn)算
t為當(dāng)前運(yùn)行時間
對于相同類型的周期信號，我們可以利用相位差來表征周期函數(shù)之間的差別，因此有了基準(zhǔn)相位函數(shù)之后，我們就可以利用其來定義各條腿的相位，如下式：
其中，Φi,offset為第條腿與基準(zhǔn)相位的相位差。
3、周期函數(shù)效果演示
這里簡單利用一個線性函數(shù)進(jìn)行演示
取a=1,b=1，其圖像隨時間變化如下：
利用周期函數(shù)對時間進(jìn)行調(diào)整，可以使其變成周期函數(shù)，設(shè)定周期為2s，時長為10s，其圖像如下：
從圖像可以看出，剛好為5個周期，一個周期為一個尖角。接下來，為更好的觀察各曲線相位，我們設(shè)定該直線函數(shù)的3個周期，其相位差（offset）分別為[0,1,3,4]，圖像如下：
從上圖可以看出，我們的“尖角”會根據(jù)相位差，向前或向后平移。此方法對所有函數(shù)均適用，以下為利用sin函數(shù)進(jìn)行的測試效果：
測試用代碼：
import numpy as npimport matplotlib.pyplot as pltT = 2*np.pit = np.linspace(0, 10, 100)offset = [0, 0.5*np.pi, np.pi, 1.5*np.pi]phi_offset = []def linear_fun(x):? ?return x+1def linear_draw():? ?plt.title('linear_function')? ?plt.plot(t, linear_fun(t))def single_period():? ?plt.title('period_linear_function')? ?plt.plot(t, linear_fun(t % T))def period_draw():? ?plt.title('period_ot')? ?for i in range(len(offset)):? ? ? ?phi_offset.append(np.sin((t+offset[i]) % T))? ? ? ?plt.plot(t, phi_offset[i], label='%f' % offset[i])period_draw()plt.legend()plt.show()
4、步態(tài)規(guī)劃
因為我們的四足機(jī)器人足端，是在不停地與地面接觸，懸空（擺動），所以這里說的步態(tài)規(guī)劃，其實指的的一個連續(xù)（離散）時間上的接觸序列。
我們以一條腿為例，本質(zhì)上，可以看作是一個周期性的，用于生成各腿接觸狀態(tài)的階躍函數(shù)。其輸入值為時間，輸出值為0或1，函數(shù)可以表示成如下：
其中t=t%T為時間的周期函數(shù)，其圖像如下所示。
可以看出，步態(tài)規(guī)劃器給出的是一段由0-1組成的接觸序列。接下來加上相位關(guān)系。以tort步態(tài)為例，其相位為[0, 0.5, 0.5, 0]，步態(tài)規(guī)劃隨時間的圖像如下所示：
測試代碼：
import numpy as npimport matplotlib.pyplot as pltT = 1t = np.linspace(0, 5, 100)states = []FR = []FL = []BR = []BL = []phi_offset = [FR, FL, BR, BL]offset = [0, 0.5, 0.5, 0]def step(t):? ?if t < 0.5*T:? ? ? ?state = 0? ?else:? ? ? ?state = 1? ?return statedef draw():? ?for time in t:? ? ? ?for i in range(len(offset)):? ? ? ? ? ?phi_offset[i].append(step((time+offset[i]) % T))? ?plt.subplot(411)? ?plt.ylabel('FR')? ?plt.plot(t, phi_offset[0])? ?plt.subplot(412)? ?plt.ylabel('FL')? ?plt.plot(t, phi_offset[1])? ?plt.subplot(413)? ?plt.ylabel('BR')? ?plt.plot(t, phi_offset[2])? ?plt.subplot(414)? ?plt.ylabel('BL')? ?plt.plot(t, phi_offset[3])fig = plt.figure()draw()fig.tight_layout()plt.show()
5、總結(jié)
綜上，我們利用階躍函數(shù)得到一串由0和1組成的序列，來表示四足機(jī)器人足端與地面的接觸狀態(tài)。根據(jù)不同的接觸狀態(tài)，再交由其他運(yùn)動控制器進(jìn)行控制，如接觸狀態(tài)，就由mpc模型計算出所需要的反作用力；而擺動狀態(tài)，就交由擺動控制模塊計算足端的擺動軌跡
最后根據(jù)需要的不同的行走姿態(tài)，確定個條腿之間的相位差，實現(xiàn)各腿的協(xié)同運(yùn)動。至此我們完成了對四足機(jī)器人步態(tài)的數(shù)學(xué)建模，更多內(nèi)容可以點擊主頁查看，點點關(guān)注，后續(xù)會有更多相關(guān)內(nèi)容。
??長按圖片 掃碼申請??
