時間：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 ∣ 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
位置約束{ 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]中提出了一種基於複合擺線形式的軌跡規劃方法，並在文獻[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
我們用python繪製該軌跡的圖像如下：
從表達式上分析，該軌跡的加速度方程是餘弦函數的倍數，在 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 = { 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
再來看位移圖像：
{ 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
t > T m 2 t>\frac{T_m}{2} t > 2 T m
相比於擺動相的足端軌跡，支撐相的設計就顯得稍微簡單。首先我們要知道兩點：
支撐相水平方向上的位移曲線與擺動相的關於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
定義步態週期爲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 φ i \varphi_i φ i
通過對軌跡方程的改進及其圖像的分析，我們最終得到了一個平滑，且衝擊較小的複合擺線軌跡
如果覺得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.
