【关于四足机器人那些事】足端轨迹规划-复合摆线轨迹

时间：2020年4月9日
对之前内容做补充，加入支撑相轨迹规划，并构造完整的的周期曲线

在四足机器人的研究中，有一个很关键的问题，就是如何减少足端在触地瞬间的冲击，避免把机器人把自己给蹬倒了？这时候就需要一个合理的足端轨迹规划。本篇将会介绍几种足端轨迹。

本文将对四足机器人的足端轨迹进行规划。将数学中的复合摆线和多项式曲线引入到足端轨迹的规划中，根据零冲击原则[2]，规划出 3 条满足要求的足端轨迹，包括：

• 复合摆线轨迹
• 八次多项式轨迹
• 分段五次多项式轨迹

本篇先介绍第一个
该轨迹是摆线方程的延伸，我们先来看什么是摆线

一、摆线

摆线，又称旋轮线、圆滚线，在数学中，摆线（Cycloid）被定义为，一个圆沿一条直线运动时，圆边界上一定点所形成的轨迹。它是一般旋轮线的一种。

看以下动图就知道摆线是如何来的：

红色线即为摆线轨迹，总结成数学公式为：

$$x=r(t-\sin t)$$

$$y=r(1-\cos t)$$

二、足端轨迹约束方程

为达到理想的步态，足端轨迹规划需要满足：

• ① 行进平稳、协调，无明显的上下波动、左右摇晃和前后冲击；
• ② 各关节没有较大冲击，特别是摆动相抬腿和落地瞬间实现零冲击抬腿和落地软
  着陆；
• ③ 摆动腿跨步迅速，足端轨迹圆滑，关节速度和加速度平滑连续无畸点；
• ④ 避免足端与地面接触时产生滑动，无摆动腿拖地现象。

规定在$0\sim \frac{T}{2}$ ，足端处于摆动相，在$\frac{T}{2}\sim T$ ，足端处于支撑相。设水平方向为 X 方向，竖直方向为 Y 方向，根据四足机器人足端运动位置的要求，可确定足端轨迹在水平方向(X 方向)和竖直方向(Y 方向)的位移方程有以下约束。

1、水平x方向

位置约束
$$\{\begin{array}{cc}x{|}_{t=0}=0& \\ x{|}_{t=T/2}=& S\\ x{|}_{t=T}=0& \end{array}$$

速度约束
$$\{\begin{array}{cc}\dot{x}{|}_{t=0}=0& \\ \dot{x}{|}_{t=T/2}=& 0\\ \dot{x}{|}_{t=T}=0& \end{array}$$

加速度约束
$$\{\begin{array}{cc}\ddot{x}{|}_{t=0}=0& \\ \ddot{x}{|}_{t=T/2}=& 0\\ \ddot{x}{|}_{t=T}=0& \end{array}$$

2、竖直y方向

位置约束
$$\{\begin{array}{ccc}& y{|}_{t=0}& =0\\ & y{|}_{t=T/4}& =H\\ & y=0,& T/2\le t\le T\end{array}$$

速度约束
$$\{\begin{array}{ccc}& \dot{y}{|}_{t=0}& =0\\ & \dot{y}{|}_{t=T/4}& =0\\ & \dot{y}=0,& T/2\le t\le T\end{array}$$

加速度约束
$$\{\begin{array}{ccc}& \ddot{y}{|}_{t=0}& =0\\ & \ddot{y}{|}_{t=T/4}& =0\\ & \ddot{y}=0,& T/2\le t\le T\end{array}$$

三、复合摆线轨迹

1、摆动相轨迹设计

基于第二节中提到的原则，文献[1]中提出了一种基于复合摆线形式的轨迹规划方法，并在文献[2]中得到了验证和使用。针对机器人足底与地面接触时会产生滑动和行走过程中拖地问题，文献[2]对复合摆线规划方法提出了修改，修改后的摆动腿的步态规划轨迹定义为：

$$\{\begin{array}{c}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{array}$$

其中S为步幅， H为抬腿高度，$T_m$为摆动相周期。现在我们来看提下它的图像，设定S = 100， H = 30， 周期T=2。

我们用python绘制该轨迹的图像如下：

为了更好地研究该轨迹的特性，我们接下来依次看x，y关于时间的位置，速度，加速度

2、轨迹改进

从表达式上分析，该轨迹的加速度方程是余弦函数的倍数，在 t=0 和 t=Tm时刻会出现加速度跳变，根据$F=Ma$，这就导致了抬腿的瞬间要求产生较大的接触力。

从加速度图像亦能说明这一点。针对这一问题，文献[5]对 y 方向的位移方程提出了以下修改。

由于摆动腿在 y 轴运动需要先抬腿再落腿，借鉴 x 轴正弦方式运动的方法求解 y 轴位移曲线

先从加速度函数入手，我们设计成如下形式：
$$\ddot{y}=A\sin (\frac{n\pi t}{T_m})$$

对上式进行积分求得速度函数：

$$\dot{y}=\frac{A{T}_m}{n\pi }\left[-\cos (\frac{n\pi t}{T_m})\right]+C_1$$

然后根据速度约束$\dot{y}{|}_{t=0}=0$以及$\dot{y}{|}_{t=T_m}=0$，求出$C_1$

$$C_1=\frac{A{T}_m}{n\pi }n=2kk=1,2,...$$

对速度函数进行积分，求得位移函数为：

$$y=\frac{A{T}_m}{n\pi }\left[t-\frac{T_m}{n\pi }\sin (\frac{n\pi t}{T_m})\right]+C_2$$

而根据轨迹约束$y{|}_{t=0}=0,y{|}_{t=T_m/2}=H,y{|}_{t=T_m}=0$我们无法求解出参数$A$和$C_2$，因此通过分段函数的方法求y轴方向的曲线方程：

$$y=\{\begin{array}{cc}2H\left[\frac{t}{T_m}-\frac{1}{n\pi }\sin (\frac{n\pi t}{T_m})\right]& 0\le 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}\le t<T_m\end{array}$$

为了确定n的取值，我们来看看n取不同值时的速度图像
取$T=2，H=1，S=1$

可以看出，当n越大时，y方向上的速度变化就越频繁，这可能会导致系统能耗的增加。

再来看位移图像：

从位移图像来看，只有当n取4时，轨迹才是平滑的。因此我们可以确定轨迹的最终形式为：

$$\{\begin{array}{cc}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{array}$$

其中
$$f_E(t)=\frac{t}{T_m}-\frac{1}{4\pi }\sin (\frac{4\pi t}{T_m})$$

$$sgn(\frac{T_m}{2}-t)=\{\begin{array}{c}10⩽t<\frac{T_m}{2}\\ \\ -1\frac{T_m}{2}⩽t<T_m\end{array}$$

我们取$H=30,S=100,T=2$来验证上式：

这里我们发现速度图像与确定n值时给出的不一致，那是因为我们在y的轨迹方程中使用了分段函数，对$t>\frac{T_m}{2}$部分进行了取反，因此该部分的速度和加速度图像是沿横轴进行了翻转的。

2020年4月9日 新增内容

3、支撑相足端轨迹

相比于摆动相的足端轨迹，支撑相的设计就显得稍微简单。首先我们要知道两点：

• 支撑相水平方向上的位移曲线与摆动相的关于$t=T_m$对称。
• 竖直方向的位移适终为0

基于这两点，我们可以设计出如下曲线：

$$\{\begin{array}{ccc}x& =& S(\frac{2T_m-t}{T_m}+\frac{1}{2\pi }\sin (\frac{2\pi t}{T_m})),T_m\le y\le 2T_m\\ & & \\ y& =& 0,T_m\le t\le 2T_m\end{array}$$

4、周期轨迹

定义步态周期为$T$，支撑相，摆动相周期均为$T_m$
，则$T=2T_m$

关于周期：摆动相$T_f$，支撑相$T_s$的持续时间不一定要设为一致，可根据控制器模型实时动态调节。这种情况下$T=T_f+T_s$

我们对时间进行周期化处理：

$$t_i=(t+{\phi }_i)mod{T}_i$$

其中，$t$为系统时间，$t_i$为第$i$条腿的轨迹规划时间，以LF腿的相位为初始值，则${\phi }_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.