時間:2020年4月9日
對之前內容做補充,加入支撐相軌跡規劃,並構造完整的的週期曲線
在四足機器人的研究中,有一個很關鍵的問題,就是如何減少足端在觸地瞬間的衝擊,避免把機器人把自己給蹬倒了?這時候就需要一個合理的足端軌跡規劃。本篇將會介紹幾種足端軌跡。
本文將對四足機器人的足端軌跡進行規劃。將數學中的複合擺線和多項式曲線引入到足端軌跡的規劃中,根據零衝擊原則[2],規劃出 3 條滿足要求的足端軌跡,包括:
本篇先介紹第一個
該軌跡是擺線方程的延伸,我們先來看什麼是擺線
一、擺線
擺線,又稱旋輪線、圓滾線,在數學中,擺線(Cycloid)被定義爲,一個圓沿一條直線運動時,圓邊界上一定點所形成的軌跡。它是一般旋輪線的一種。
看以下動圖就知道擺線是如何來的:
紅色線即爲擺線軌跡,總結成數學公式爲:
x = r ( t − sin t ) x = r(t-\sin t) x = r ( t − sin t )
y = r ( 1 − cos t ) y = r(1-\cos t) y = r ( 1 − cos t )
二、足端軌跡約束方程
爲達到理想的步態,足端軌跡規劃需要滿足:
① 行進平穩、協調,無明顯的上下波動、左右搖晃和前後衝擊;
② 各關節沒有較大沖擊,特別是擺動相擡腿和落地瞬間實現零衝擊擡腿和落地軟
着陸;
③ 擺動腿跨步迅速,足端軌跡圓滑,關節速度和加速度平滑連續無畸點;
④ 避免足端與地面接觸時產生滑動,無擺動腿拖地現象。
規定在0 ∼ T 2 0 \sim \frac{T}{2} 0 ∼ 2 T ,足端處於擺動相,在T 2 ∼ T \frac{T}{2} \sim T 2 T ∼ T ,足端處於支撐相。設水平方向爲 X 方向,豎直方向爲 Y 方向,根據四足機器人足端運動位置的要求,可確定足端軌跡在水平方向(X 方向)和豎直方向(Y 方向)的位移方程有以下約束。
1、水平x方向
位置約束
{ x ∣ t = 0 = 0 x ∣ t = T / 2 = S x ∣ t = T = 0 \left\{\begin{matrix}
x|_{t=0} = 0\\
x|_{t=T/2} =& S\\
x|_{t = T} = 0
\end{matrix}\right. ⎩ ⎨ ⎧ x ∣ t = 0 = 0 x ∣ t = T / 2 = x ∣ t = T = 0 S
速度約束
{ x ˙ ∣ t = 0 = 0 x ˙ ∣ t = T / 2 = 0 x ˙ ∣ t = T = 0 \left\{\begin{matrix}
\dot{x}|_{t=0} = 0\\
\dot{x}|_{t=T/2} =& 0\\
\dot{x}|_{t = T} = 0
\end{matrix}\right. ⎩ ⎨ ⎧ x ˙ ∣ t = 0 = 0 x ˙ ∣ t = T / 2 = x ˙ ∣ t = T = 0 0
加速度約束
{ x ¨ ∣ t = 0 = 0 x ¨ ∣ t = T / 2 = 0 x ¨ ∣ t = T = 0 \left\{\begin{matrix}
\ddot{x}|_{t=0} = 0\\
\ddot{x}|_{t=T/2} =& 0\\
\ddot{x}|_{t = T} = 0
\end{matrix}\right. ⎩ ⎨ ⎧ x ¨ ∣ t = 0 = 0 x ¨ ∣ t = T / 2 = x ¨ ∣ t = T = 0 0
2、豎直y方向
位置約束
{ y ∣ t = 0 = 0 y ∣ t = T / 4 = H y = 0 , T / 2 ≤ t ≤ T \left\{\begin{matrix}
&y|_{t=0}& = 0\\
&y|_{t=T/4}& = H \\
&y = 0,&T/2\leq t\leq T
\end{matrix}\right. ⎩ ⎨ ⎧ y ∣ t = 0 y ∣ t = T / 4 y = 0 , = 0 = H T / 2 ≤ t ≤ T
速度約束
{ y ˙ ∣ t = 0 = 0 y ˙ ∣ t = T / 4 = 0 y ˙ = 0 , T / 2 ≤ t ≤ T \left\{\begin{matrix}
&\dot{y}|_{t=0}& = 0\\
&\dot{y}|_{t=T/4}& = 0 \\
&\dot{y} = 0,&T/2\leq t\leq T
\end{matrix}\right. ⎩ ⎨ ⎧ y ˙ ∣ t = 0 y ˙ ∣ t = T / 4 y ˙ = 0 , = 0 = 0 T / 2 ≤ t ≤ T
加速度約束
{ y ¨ ∣ t = 0 = 0 y ¨ ∣ t = T / 4 = 0 y ¨ = 0 , T / 2 ≤ t ≤ T \left\{\begin{matrix}
&\ddot{y}|_{t=0}& = 0\\
&\ddot{y}|_{t=T/4}& = 0 \\
&\ddot{y} = 0,&T/2\leq t\leq T
\end{matrix}\right. ⎩ ⎨ ⎧ y ¨ ∣ t = 0 y ¨ ∣ t = T / 4 y ¨ = 0 , = 0 = 0 T / 2 ≤ t ≤ T
三、複合擺線軌跡
1、擺動相軌跡設計
基於第二節中提到的原則,文獻[1]中提出了一種基於複合擺線形式的軌跡規劃方法,並在文獻[2]中得到了驗證和使用。針對機器人足底與地面接觸時會產生滑動和行走過程中拖地問題,文獻[2]對複合擺線規劃方法提出了修改,修改後的擺動腿的步態規劃軌跡定義爲:
{ x = S [ t T m − 1 2 π sin ( 2 π t T m ) ] y = H [ 1 2 − 1 2 cos ( 2 π t T m ) ] \left\{\begin{matrix}
x = S\left [\frac{t} {T_m}-\frac{1}{2\pi}\sin(\frac{2\pi t}{T_m}) \right ]\\ \\
y = H\left [ \frac{1}{2}-\frac{1}{2}\cos(\frac{2\pi t}{T_m})\right ]
\end{matrix}\right. ⎩ ⎪ ⎪ ⎨ ⎪ ⎪ ⎧ x = S [ T m t − 2 π 1 sin ( T m 2 π t ) ] y = H [ 2 1 − 2 1 cos ( T m 2 π t ) ]
其中S爲步幅, H爲擡腿高度,T m T_m T m 爲擺動相週期。現在我們來看提下它的圖像,設定S = 100, H = 30, 週期T=2。
我們用python繪製該軌跡的圖像如下:
爲了更好地研究該軌跡的特性,我們接下來依次看x,y關於時間的位置,速度,加速度
2、軌跡改進
從表達式上分析,該軌跡的加速度方程是餘弦函數的倍數,在 t=0 和 t=Tm時刻會出現加速度跳變,根據F = M a F=Ma F = M a ,這就導致了擡腿的瞬間要求產生較大的接觸力。
從加速度圖像亦能說明這一點。針對這一問題,文獻[5]對 y 方向的位移方程提出了以下修改。
由於擺動腿在 y 軸運動需要先擡腿再落腿,借鑑 x 軸正弦方式運動的方法求解 y 軸位移曲線
先從加速度函數入手,我們設計成如下形式 :
y ¨ = A sin ( n π t T m ) \ddot{y} = A\sin(\frac{n\pi t}{T_m}) y ¨ = A sin ( T m n π t )
對上式進行積分求得速度函數 :
y ˙ = A T m n π [ − cos ( n π t T m ) ] + C 1 \dot{y} = \frac{AT_m}{n\pi}\left [ -\cos(\frac{n\pi t}{T_m})\right ] +C_1 y ˙ = n π A T m [ − cos ( T m n π t ) ] + C 1
然後根據速度約束y ˙ ∣ t = 0 = 0 \dot{y}|_{t=0} = 0 y ˙ ∣ t = 0 = 0 以及y ˙ ∣ t = T m = 0 \dot{y}|_{t=T_m} = 0 y ˙ ∣ t = T m = 0 ,求出C 1 C_1 C 1
C 1 = A T m n π n = 2 k k = 1 , 2 , . . . C_1 = \frac{AT_m}{n\pi} \quad n=2k \quad k=1,2,... C 1 = n π A T m n = 2 k k = 1 , 2 , . . .
對速度函數進行積分,求得位移函數爲:
y = A T m n π [ t − T m n π sin ( n π t T m ) ] + C 2 y = \frac{AT_m}{n\pi}\left [t -\frac{T_m}{n\pi}\sin(\frac{n\pi t}{T_m})\right ] +C_2 y = n π A T m [ t − n π T m sin ( T m n π t ) ] + C 2
而根據軌跡約束y ∣ t = 0 = 0 , y ∣ t = T m / 2 = H , y ∣ t = T m = 0 y|_{t=0} = 0, y|_{t=T_m/2} = H, y|_{t=T_m} = 0 y ∣ t = 0 = 0 , y ∣ t = T m / 2 = H , y ∣ t = T m = 0 我們無法求解出參數A A A 和C 2 C_2 C 2 ,因此通過分段函數的方法求y軸方向的曲線方程:
y = { 2 H [ t T m − 1 n π sin ( n π t T m ) ] 0 ≤ t < T m 2 2 H [ 1 − t T m + 1 n π sin ( n π t T m ) ] T m 2 ≤ t < T m y = \left\{\begin{matrix}
2H\left [ \frac{t}{T_m}-\frac{1}{n\pi}\sin(\frac{n\pi t}{T_m})\right ] & 0\leq t< \frac{T_m}{2}\\ \\
2H\left [1 - \frac{t}{T_m}+\frac{1}{n\pi}\sin(\frac{n\pi t}{T_m}) \right ] & \frac{T_m}{2}\leq t< T_m
\end{matrix}\right. y = ⎩ ⎪ ⎪ ⎨ ⎪ ⎪ ⎧ 2 H [ T m t − n π 1 sin ( T m n π t ) ] 2 H [ 1 − T m t + n π 1 sin ( T m n π t ) ] 0 ≤ t < 2 T m 2 T m ≤ t < T m
爲了確定n的取值,我們來看看n取不同值時的速度圖像
取T = 2 , H = 1 , S = 1 T = 2,H = 1, S = 1 T = 2 , H = 1 , S = 1
可以看出,當n越大時,y方向上的速度變化就越頻繁,這可能會導致系統能耗的增加。
再來看位移圖像:
從位移圖像來看,只有當n取4時,軌跡纔是平滑的。因此我們可以確定軌跡的最終形式爲:
{ x = S [ t T m − 1 2 π sin ( 2 π t T m ) ] y = H [ s g n ( T m 2 − t ) ( 2 f E ( t ) − 1 ) + 1 ] \left\{\begin{matrix}
x =& S\left [\frac{t}{T_m}-\frac{1}{2\pi}\sin(\frac{2\pi t}{T_m}) \right ]\\ \\
y =& H\left [ sgn(\frac{T_m}{2}-t)(2f_E(t)-1) + 1 \right ]
\end{matrix}\right. ⎩ ⎪ ⎨ ⎪ ⎧ x = y = S [ T m t − 2 π 1 sin ( T m 2 π t ) ] H [ s g n ( 2 T m − t ) ( 2 f E ( t ) − 1 ) + 1 ]
其中
f E ( t ) = t T m − 1 4 π sin ( 4 π t T m ) f_E(t) = \frac{t}{T_m}-\frac{1}{4\pi}\sin(\frac{4\pi t}{T_m}) f E ( t ) = T m t − 4 π 1 sin ( T m 4 π t )
s g n ( T m 2 − t ) = { 1 0 ⩽ t < T m 2 − 1 T m 2 ⩽ t < T m sgn(\frac{T_m}{2}-t) = \left\{\begin{matrix}
1 \quad 0\leqslant t < \frac{T_m}{2}\\ \\
-1 \quad \frac{T_m}{2}\leqslant t < T_m
\end{matrix}\right. s g n ( 2 T m − t ) = ⎩ ⎨ ⎧ 1 0 ⩽ t < 2 T m − 1 2 T m ⩽ t < T m
我們取H = 30 , S = 100 , T = 2 H=30, S=100, T=2 H = 3 0 , S = 1 0 0 , T = 2 來驗證上式:
這裏我們發現速度圖像與確定n值時給出的不一致,那是因爲我們在y的軌跡方程中使用了分段函數,對t > T m 2 t>\frac{T_m}{2} t > 2 T m 部分進行了取反,因此該部分的速度和加速度圖像是沿橫軸進行了翻轉的。
3、支撐相足端軌跡
相比於擺動相的足端軌跡,支撐相的設計就顯得稍微簡單。首先我們要知道兩點:
支撐相水平方向上的位移曲線與擺動相的關於t = T m t=T_m t = T m 對稱。
豎直方向的位移適終爲0
基於這兩點,我們可以設計出如下曲線:
{ x = S ( 2 T m − t T m + 1 2 π sin ( 2 π t T m ) ) , T m ≤ y ≤ 2 T m y = 0 , T m ≤ t ≤ 2 T m \left\{\begin{matrix}
x &=& S(\frac{2T_m -t}{T_m} + \frac{1}{2\pi}\sin(\frac{2\pi t}{T_m})), T_m \leq y \leq 2T_m\\ \\
y &=& 0, T_m \leq t \leq 2T_m
\end{matrix}\right. ⎩ ⎨ ⎧ x y = = S ( T m 2 T m − t + 2 π 1 sin ( T m 2 π t ) ) , T m ≤ y ≤ 2 T m 0 , T m ≤ t ≤ 2 T m
4、週期軌跡
定義步態週期爲T T T ,支撐相,擺動相週期均爲T m T_m T m
,則T = 2 T m T = 2T_m T = 2 T m
關於週期:擺動相T f T_f T f ,支撐相T s T_s T s 的持續時間不一定要設爲一致,可根據控制器模型實時動態調節。這種情況下T = T f + T s T = T_f + T_s T = T f + T s
我們對時間進行週期化處理:
t i = ( t + φ i ) m o d T i t_i = (t+ \varphi_i)modT_i t i = ( t + φ i ) m o d T i
其中,t t t 爲系統時間,t i t_i t i 爲第i i i 條腿的軌跡規劃時間,以LF腿的相位爲初始值,則φ i \varphi_i φ i 爲各腿相位落後於LF腿的時間與步態週期的比值。
總結
通過對軌跡方程的改進及其圖像的分析,我們最終得到了一個平滑,且衝擊較小的複合擺線軌跡
如果覺得ok,點個贊,點個關注, 也歡迎給個打賞 支持一下編者的工作
參考文獻
[1] SAKAKIBARA Y,KAN K,HOSODA Y,et al. Foot trajectory for a quadruped walking machine[C] // Proceedings IROS '90. IEEE International Workshop on,July 3-6,1990,Ibaraki,Japan. New York,NY,USA:IEEE,1990:315-322.
[2] 何冬青,馬培蓀. 四足機器人動態步行仿真及步行穩定性分析[J]. 計算機仿真,2005(2):146-149. HE Dongqing , MA Peisun. Simulation of dynamic walking of quadruped robot and analysis of walking stability[J]. Computer Simulation,2005(2):146-149.
[3] 李貽斌,李彬,榮學文,等. 液壓驅動四足仿生機器人的結構設計和步態規劃[J]. 山東大學學報,2011(5):32-36,45. LI Yibin,LI Bin,RONG Xuewen,et al. Mechanical design and gait planning of a hydraulically actuated quadruped bionic robot[J]. Journal of Shandong University,2011(5):32-36,45.