本篇将讲解MiniCheetah中所使用的的足端轨迹方程-贝塞尔曲线方程

一、贝塞尔曲线

贝塞尔曲线就是这样的一条曲线，它是依据四个位置任意的点坐标绘制出的一条光滑曲线。

在历史上，研究贝塞尔曲线的人最初是按照已知曲线参数方程来确定四个点的思路设计出这种矢量曲线绘制法。贝塞尔曲线的有趣之处更在于它的“皮筋效应”，也就是说，随着点有规律地移动，曲线将产生皮筋伸引一样的变换，带来视觉上的冲击。1962年，法国数学家Pierre Bézier第一个研究了这种矢量绘制曲线的方法，并给出了详细的计算公式，因此按照这样的公式绘制出来的曲线就用他的姓氏来命名是为贝塞尔曲线。

二、数学形式

1、线性公式

给定点P0、P1，线性贝兹曲线只是一条两点之间的直线。这条线由下式给出：

B ( t ) = P 0 + ( P 1 − P 0 ) t = ( 1 − t ) P 0 + t P 1 , t ∈ [ 0 , 1 ] B(t) = P_0 + (P_1 -P_0)t = (1-t)P_0+tP_1,t \in [0, 1] B(t)=P0+(P1−P0)t=(1−t)P0+tP1,t∈[0,1]

取 P 0 = [ 0 , 0 ] , P 1 = [ 1 , 1 ] P_0 = [0, 0], P_1 = [1, 1] P0=[0,0],P1=[1,1]。可以从图中看出，其效果等同于线性插值。

2、二次方公式

对于给定点 P 0 = [ 0 , 0 ] 、 P 1 = [ 0.5 , 0.5 ] 、 P 2 = [ 1 , 0 ] P0=[0, 0]、P1=[0.5, 0.5]、P2=[1, 0] P0=[0,0]、P1=[0.5,0.5]、P2=[1,0]，曲线的函数如下：

B ( t ) = ( 1 − t ) 2 P 0 + 2 t ( 1 − t ) P 1 + t 2 P 2 , t ∈ [ 0 , 1 ] B(t) = (1-t)^2P_0 + 2t(1-t)P_1 + t^2P_2,t \in [0, 1] B(t)=(1−t)2P0+2t(1−t)P1+t2P2,t∈[0,1]

3、三次方公式

P0、P1、P2、P3四个点在平面或在三维空间中定义了三次方贝兹曲线。曲线起始于P0走向P1，并从P2的方向来到P3。一般不会经过P1或P2；这两个点只是在那里提供方向信息。P0和P1之间的间距，决定了曲线在转而趋进P3之前，走向P2方向的“长度有多长”。函数形式如下：

B ( t ) = P 0 ( 1 − t ) 3 + 3 P 1 t ( 1 − t ) 2 + 3 P 2 t 2 ( 1 − t ) + P 3 t 3 , t ∈ [ 0 , 1 ] B(t) = P_0(1-t)^3 + 3P_1t(1-t)^2 + 3P_2t^2(1-t)+P_3t^3, t \in [0, 1] B(t)=P0(1−t)3+3P1t(1−t)2+3P2t2(1−t)+P3t3,t∈[0,1]

三、Cheetah中所使用的轨迹函数

这里我们先着眼于位置，把源码中的速度项，加速项去掉，对其进行分析。

1.1 代码部分

轨迹生成函数调用：

template <typename T>
void FootSwingTrajectory<T>::computeSwingTrajectoryBezier(T phase, T swingTime) {_p = Interpolate::cubicBezier<Vec3<T>>(_p0, _pf, phase);T zp, zv, za;if(phase < T(0.5)) {zp = Interpolate::cubicBezier<T>(_p0[2], _p0[2] + _height, phase * 2);} else {zp = Interpolate::cubicBezier<T>(_p0[2] + _height, _pf[2], phase * 2 - 1);}_p[2] = zp;
}

方法实现

template <typename y_t, typename x_t>
y_t cubicBezier(y_t y0, y_t yf, x_t x) {static_assert(std::is_floating_point<x_t>::value,"must use floating point value");assert(x >= 0 && x <= 1);y_t yDiff = yf - y0;x_t bezier = x * x * x + x_t(3) * (x * x * (x_t(1) - x));return y0 + bezier * yDiff;
}

1.2 数学模型

1.2.1 空间轨迹

规定以下符号

任意起始点 p 0 = [ x 0 y 0 z 0 ] T p_0=[x_0 \quad y_0 \quad z_0]^T p0=[x0y0z0]T
结束点 p f = [ x f y f z f ] T p_f = [x_f \quad y_f \quad z_f]^T pf=[xfyfzf]T
抬腿高度h。

将方法实现中的代码转换成数学表达式如下：

Δ p = p f − p 0 \Delta p = p_f - p_0 Δp=pf−p0

b = t 3 + 3 t 2 ( 1 − t ) b = t^3 + 3t^2(1-t) b=t3+3t2(1−t)

p o u t = p 0 + b Δ p p_{out} = p_0 + b\Delta p pout=p0+bΔp

将上式合并成一条：

p o u t = p 0 + ( t 3 + 3 t 2 ( 1 − t ) ) ( p f − p 0 ) (1) p_{out}= p_0+ (t^3+3t^2(1-t))(p_f - p_0) \tag{1} pout=p0+(t3+3t2(1−t))(pf−p0)(1)

需要注意的是，该计算可针对一维变量(例如对Z轴坐标单独计算)，也可以是三维向量之间的运算(例如坐标点)，对于三维点计算，其展开形式如下(实际上与式1无异)：

[ x o u t y o u t z o u t ] = [ x 0 y 0 z 0 ] + ( t 3 + 3 t 2 ( 1 − t ) ) [ x f − x 0 y f − y 0 z f − z 0 ] \begin{bmatrix}x_{out}\\ y_{out}\\ z_{out}\end{bmatrix} = \begin{bmatrix}x_0\\ y_0\\ z_0\end{bmatrix}+ (t^3+3t^2(1-t))\begin{bmatrix} x_f - x_0\\ y_f - y_0\\ z_f - z_0 \end{bmatrix} ⎣⎡xoutyoutzout⎦⎤=⎣⎡x0y0z0⎦⎤+(t3+3t2(1−t))⎣⎡xf−x0yf−y0zf−z0⎦⎤

下面结合图像更加直观地对其进行分析。先分析式1的图像，在 t ∈ [ 0 , 1 ] t \in [0, 1] t∈[0,1] 的情况下，分别取两组数据：

p 0 = 0.5 p_0=0.5 p0=0.5
p f = 0.2 p_f=0.2 pf=0.2

p 0 , p f p_0,p_f p0,pf的值互换

p 0 = 0.2 p_0=0.2 p0=0.2
p f = 0.5 p_f=0.5 pf=0.5

可以看到如下图像：

因此可以得出结论，源码中，该部分定义了，摆动相上行与下行的轨迹。：

  if(phase < T(0.5)) {zp = Interpolate::cubicBezier<T>(_p0[2], _p0[2] + _height, phase * 2);} else {zp = Interpolate::cubicBezier<T>(_p0[2] + _height, _pf[2], phase * 2 - 1);}

下面来看整体轨迹，设定以下数据：

起始点 p 0 = [ 0 , 0 , 0.2 ] p_0=[0, 0, 0.2] p0=[0,0,0.2]
结束点 p f = [ 1.5 , 1.5 , 0 ] p_f=[1.5, 1.5, 0] pf=[1.5,1.5,0]
抬腿高度 h = 0.5 h=0.5 h=0.5

空间轨迹如下：

各轴坐标随时间的图像：

1.2.2 速度

式（1）对时间进行进行微分，得到：

p ˙ o u t = ( 3 t 2 + 6 t − 9 t 2 ) ( p f − p 0 ) \dot p_{out}= (3t^2+6t - 9t^2)(p_f - p_0) p˙out=(3t2+6t−9t2)(pf−p0)

化简变成：

p ˙ o u t = 6 t ( 1 − t ) ( p f − p 0 ) (2) \dot p_{out}= 6t(1 - t)(p_f - p_0) \tag{2} p˙out=6t(1−t)(pf−p0)(2)

1.2.3 加速度

对式（2）微分，求得：

p ¨ o u t = ( 6 − 12 t ) ( p f − p 0 ) (3) \ddot p_{out}= (6 - 12t)(p_f - p_0) \tag{3} p¨out=(6−12t)(pf−p0)(3)

从公式可以看出，此时加速度方程为线性方程：

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

赤兔四足机器人的作用_跑得快，打不死！清华大学开发“小强”机器人，壮汉狂踩也挡不住前进步伐...
大数据文摘编辑部出品提到蟑螂,很多同学都深恶痛绝. 这种身型小巧的虫子不仅跑得快.繁殖能力强,而且超级抗打抗压,在所有的环境下都能顽强地生存下去. 12mm高的蟑螂可以躲进4mm的缝隙也难怪周星驰 ...
史上最全 2019 ICRA顶会四足机器人文献整理
史上最全 2019 ICRA顶会四足机器人文献整理一.ICRA论文集中相关文献对应subsession时间二.文献整理内容一.ICRA论文集中相关文献对应subsession时间 15:15-1 ...
浙江大学发布四足机器人“绝影”，爬坡踏雪稳定性令人惊艳
在视频中,"绝影"展现了反应快.姿态稳和定位准的特性. 日前,浙江大学熊蓉教授领导的机器人团队发布了一段四足机器人的视频.视频的主角是一个名为"绝影"的四足机器 ...
四足论文《面对未知地形的四足机器人足端轨迹优化》解读
在笔者的关于足端轨迹规划的文章中,曾经提到了类正弦轨迹,该规划是根据斯坦福的四足机器人Doggo的开源代码中得出的.而该轨迹的缺点是对于地形的适应差,无法自适应的调节自身足端的轨迹规划. 基于该种问题 ...
不用摄像头和激光雷达，四足机器人「凭感觉」越野：ANYmal登上Science子刊封面...
本文经机器之心(almosthuman2014)授权转载,禁止二次转载. 近日,瑞士 ANYbotics 公司打造的 ANYmal 机器人登上了新一期的<Science Robotics> ...
小米CyberDog四足机器人的AI运动系统的实现
2021年8月10日,雷军进行继宣布造车之后的第二次演讲. 在现场的展示中,小米发布了首款四足行走机器人--铁蛋,英文名CyberDog.这只"人类高质量宠物"可以行走.站立.小碎 ...
Science Robotics：新型多足机器人可自行组装，零件损坏时也能继续运动
来源:DeepTech深科技在自然环境中,群体昆虫(如蜜蜂.蚂蚁.白蚁等).鱼类和鸟类等动物可以通过合作来完成生物个体难以或不可能完成的任务.受到这些集群行为的启发,许多研究人员研究了自组装或可重构 ...
六足机器人的实现原理
缘由: 在自然界和人类社会中存在一些人类无法到达的地方和可能危及人类生命的特殊场合.如行星表面.灾难发生矿井.防灾救援和反恐斗争等,对这些危险环境进行不断地探索和研究,寻求一条解决问题的可行途径成为科 ...
气动骨骼肌两足机器人的设计与控制
Design and control of a pneumatic musculoskeletal biped robot Xizhe Zang∗, Yixiang Liu, Xinyu Liu an ...

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