系列文章目录

【四足机器人那些事儿2】MiniCheetah中所使用的的足端轨迹方程

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

文章目录

系列文章目录
前言
一、多项式曲线拟合
- 1、一次多项式
- 2、二次多项式
- 3、多次多项式
二、足端轨迹约束方程
- 1、水平x方向
- 2、竖直y方向
三、八次多项式曲线
- 1、X方向
- 2、Y方向
总结
- X方向轨迹方程
- Y方向轨迹方程
- 动态轨迹

前言

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

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

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

本篇先介绍 - 八次多项式轨迹

一、多项式曲线拟合

1、一次多项式

众所周知，过任意不同的两点可以确定一条直线。即已知：

P1=[x1,y1]P2=[x2,y2]\begin{aligned} &P_1 = [x_1, y_1] \\ &P_2 = [x_2, y_2] \end{aligned}P1=[x1,y1]P2=[x2,y2]

可求得用两点式表示直线方程：

x−x1x2−x1=y−y1y2−y1\frac{x-x_1}{x_2-x_1} = \frac{y-y_1}{y_2-y_1}x2−x1x−x1=y2−y1y−y1

该式可在带入坐标点后化简得到其点斜式形式，也就是一条一次的多项式，这里我们可以称已知的两个点坐标为方程的约束：

y=ax+by = ax + by=ax+b

2、二次多项式

加入给定任意不同的三个点，我们能否确定一条曲线通过这些点，答案是可以的。已知

P1=[x1,y1]P2=[x2,y2]P3=[x3,y3]\begin{aligned} &P_1 = [x_1, y_1] \\ &P_2 = [x_2, y_2] \\ &P_3 = [x_3, y_3] \end{aligned}P1=[x1,y1]P2=[x2,y2]P3=[x3,y3]

根据初高中的知识，我们知道通过这三点的曲线是一条抛物线，因此我们可以假设该曲线方程为
y=ax2+bx+cy = ax^2 + bx + cy=ax2+bx+c

将点坐标带入当成即可求解得到该二次方程，对于该方程，存在3个位置点的约束。

y1=ax12+bx1+cy2=ax22+bx2+cy3=ax32+bx2+c\begin{aligned} &y_1 = ax_1^2 + bx_1 + c \\ &y_2 = ax_2^2 + bx_2 + c \\ &y_3 = ax_3^2 + bx_2 + c \end{aligned}y1=ax12+bx1+cy2=ax22+bx2+cy3=ax32+bx2+c

3、多次多项式

将上述推广到一般情况，对于给定n个不同坐标点，我们可以确定一条n-1次多项式的曲线，我们可以利用该特性来设计一种四足机器人足端轨迹方程，即在满足约束条件下的曲线方程。

二、足端轨迹约束方程

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

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

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

1、水平x方向

位置约束：初始位置为0，摆动相结束位置为0，支撑相结束位置为0
x(0)=0x(T/2)=Sx(T)=0\begin{aligned} &x(0) = 0\\ &x(T/2) = S\\ &x(T) = 0 \end{aligned}x(0)=0x(T/2)=Sx(T)=0

速度约束
x˙(0)=0x˙(T/2)=0x˙(T)=0\begin{aligned} &\dot x(0) = 0\\ &\dot x(T/2) =0\\ &\dot x(T) = 0 \end{aligned}x˙(0)=0x˙(T/2)=0x˙(T)=0

加速度约束
x¨(0)=0x¨(T/2)=0x¨(T)=0\begin{aligned} &\ddot x(0) = 0\\ &\ddot x(T/2) =0\\ &\ddot x(T) = 0 \end{aligned}x¨(0)=0x¨(T/2)=0x¨(T)=0

2、竖直y方向

位置约束
y(0)=0y(T/4)=Hy=0,T/2≤t≤T\begin{aligned} &y(0) = 0\\ &y(T/4) = H \\ &y = 0,T/2\leq t\leq T \end{aligned}y(0)=0y(T/4)=Hy=0,T/2≤t≤T

速度约束
y˙(0)=0y˙(T/4)=0y˙=0,T/2≤t≤T\begin{aligned} &\dot y(0) = 0\\ &\dot y(T/4)= 0 \\ &\dot y = 0,T/2\leq t\leq T \end{aligned}y˙(0)=0y˙(T/4)=0y˙=0,T/2≤t≤T

加速度约束
y¨(0)=0y¨(T/4)=0y¨=0,T/2≤t≤T\begin{aligned} &\ddot y(0) = 0\\ &\ddot y(T/4) = 0 \\ &\ddot{y} = 0,T/2\leq t\leq T \end{aligned}y¨(0)=0y¨(T/4)=0y¨=0,T/2≤t≤T

三、八次多项式曲线

1、X方向

根据第二节所提，对于水平方向(X 方向)，约束条件有 6 个，则设水平方向足端轨迹为五次多项式：

说明：在一个周期内（摆动相+支撑相），由于X方向上进行了一次往复运动，而支撑相运动实际上相当于摆动相的反方向运动，因此两者的约束是一致的，所以我们只需要计算摆动相时的约束即可，即0−T/20-T/20−T/2时间的运动

x=at5+bt4+ct3+dt2+et+fx˙=5at4+4bt3+3ct2+2dt+ex¨=10at3+6bt2+3ct+2d\begin{aligned} &x = at^5 + bt^4 + ct^3 + dt^2 + et + f \\ &\dot x = 5at^4 + 4bt^3 + 3ct^2 + 2dt+e \\ &\ddot x= 10at^3 + 6bt^2 + 3ct + 2d \end{aligned}x=at5+bt4+ct3+dt2+et+fx˙=5at4+4bt3+3ct2+2dt+ex¨=10at3+6bt2+3ct+2d

我们设摆动相周期为T0T_0T0，带入以下约束方程，

x(0)=0x(T0)=Sx˙(0)=0x˙(T0)=0x¨(0)=0x¨(T0)=0\begin{aligned} &x(0) = 0 &x(T_0) = S \\ &\dot x(0) = 0 &\dot x(T_0) = 0 \\ &\ddot x(0) = 0 &\ddot x(T_0) = 0 \end{aligned}x(0)=0x˙(0)=0x¨(0)=0x(T0)=Sx˙(T0)=0x¨(T0)=0

可得

a=6ST05b=−15ST04c=10ST03d=e=f=0\begin{aligned} &a = \frac{6S}{T_0^5}\\ \\ &b = -\frac{15S}{T_0^4}\\ \\ &c = \frac{10S}{T_0^3}\\ \\ &d = e = f =0 \end{aligned}a=T056Sb=−T0415Sc=T0310Sd=e=f=0

最终

x=6ST05t5−15ST04t4+10ST03t3x = \frac{6S}{T_0^5}t^5 -\frac{15S}{T_0^4}t^4 + \frac{10S}{T_0^3}t^3x=T056St5−T0415St4+T0310St3

其一阶导二阶导分别为：

x˙=30ST03t2−60ST04t3+30ST05t4\dot x = \frac{30S}{T_0^3}t^2 - \frac{60S}{T_0^4}t^3 + \frac{30S}{T_0^5}t^4x˙=T0330St2−T0460St3+T0530St4

x¨=60ST03t−180ST04t2+120ST05t3\ddot x = \frac{60S}{T_0^3}t - \frac{180S}{T_0^4}t^2 + \frac{120S}{T_0^5}t^3x¨=T0360St−T04180St2+T05120St3

设置摆动相周期为1，步长100，抬腿高度30，其位置，速度，加速度曲线如下：

2、Y方向

竖直方向上有9个约束，因次Y方向上的轨迹函数为8次多项式：

y=At8+Bt7+Ct6+Dt5+Et4+Ft3+Gt2+Ht+Iy˙=8At7+7Bt6+6Ct5+5Dt4+4Et3+3Ft2+2Gt+Hy¨=56At6+42Bt5+30Ct4+20Dt3+12Et2+6Ft+2G\begin{aligned} &y = At^8 + Bt^7 + Ct^6 + Dt^5 + Et^4 + Ft^3 + Gt^2 + Ht + I \\ &\dot y = 8At^7 + 7Bt^6 + 6Ct^5 + 5Dt^4 + 4Et^3 + 3Ft^2 + 2Gt + H \\ &\ddot y = 56At^6 + 42Bt^5 + 30Ct^4 + 20Dt^3 + 12Et^2 + 6Ft + 2G \end{aligned}y=At8+Bt7+Ct6+Dt5+Et4+Ft3+Gt2+Ht+Iy˙=8At7+7Bt6+6Ct5+5Dt4+4Et3+3Ft2+2Gt+Hy¨=56At6+42Bt5+30Ct4+20Dt3+12Et2+6Ft+2G

带入约束

这里手算比较复杂，我们用sympy进行求解：

from sympy import *A, B, C, D, E, F, T, H = symbols('A B C D E F T H')f1 = A*T**8 + B*T**7 + C*T**6 + D*T**5 + E*T**4 + F*T**3
f2 = 8*A*T**7 + 7*B*T**6 + 6*C*T**5 + 5*D*T**4 + 4*E*T**3 + 3*F*T**2
f3 = 56*A*T**6 + 42*B*T**5 + 30*C*T**4 + 20*D*T**3 + 12*E*T**2 + 6*F*T
f4 = -H + A*(T**8/256) + B*(T**7/128) + C*(T/2)**6 + D*(T/2)**5 + E*(T/2)**4 + F*(T/2)**3
f5 = 8*A*(T/2)**7 + 7*B*(T/2)**6 + 6*C*(T/2)**5 + 5*D*(T/2)**4 + 4*E*(T/2)**3 + 3*F*(T/2)**2
f6 = 56*A*(T/2)**6 + 42*B*(T/2)**5 + 30*C*(T/2)**4 + 20*D*(T/2)**3 + 12*E*(T/2)**2 + 6*F*(T/2)result = solve([f1, f2, f3, f4, f5, f6], [A, B, C, D, E, F])
print(result)

得到：

{A: -768*H/T**8, B: 3072*H/T**7, C: -4864*H/T**6, D: 3840*H/T**5, E: -1536*H/T**4, F: 256*H/T**3}

即：

y=−768HT08t8+3072HT07t7−4868HT06t6+3840HT05t5−1536HT04t4+256HT03t3y = -\frac{768H}{T_0^8}t^8 + \frac{3072H}{T_0^7}t^7-\frac{4868H}{T_0^6}t^6 + \frac{3840H}{T_0^5}t^5 - \frac{1536H}{T_0^4}t^4 + \frac{256H}{T_0^3}t^3y=−T08768Ht8+T073072Ht7−T064868Ht6+T053840Ht5−T041536Ht4+T03256Ht3

其一阶导跟二阶导分别为：

y˙=768HT03t2−6144HT04t3+19200HT5t4−29184HT06t5+21504HT07t6−6144HT08t7\dot y = \frac{768H}{T_0^3}t^2 - \frac{6144H}{T_0^4}t^3 + \frac{19200H}{T^5}t^4 - \frac{29184H}{T_0^6}t^5 + \frac{21504H}{T_0^7}t^6 - \frac{6144H}{T_0^8}t^7y˙=T03768Ht2−T046144Ht3+T519200Ht4−T0629184Ht5+T0721504Ht6−T086144Ht7

y¨=1536HT03t−18432HT04t2+76800HT05t3−145920HT06t4+129024HT07t5−43008HT08t6\ddot y = \frac{1536H}{T_0^3}t - \frac{18432H}{T_0^4}t^2 + \frac{76800H}{T_0^5}t^3 - \frac{145920H}{T_0^6}t^4 + \frac{129024H}{T_0^7}t^5 - \frac{43008H}{T_0^8}t^6y¨=T031536Ht−T0418432Ht2+T0576800Ht3−T06145920Ht4+T07129024Ht5−T0843008Ht6