单腿跳跃机器人仿真设计

先上效果视频：

抗干扰https://www.zhihu.com/video/1189263773289267200 越障https://www.zhihu.com/video/1189264317051252736 楼梯https://www.zhihu.com/video/1189264368456806400 速跑https://www.zhihu.com/video/1189264398798315520

github地址：

YuXianYuan/Hopping-in-Three-Dimensions

翻译讨论群

本文采用webots2019b进行仿真，算法主要参考Marc Raibert 的“legged robots that balance”第二、三章。

1、模型

机身高0.1m，直径0.6m，重15kg
腿最大长度1.2m，初始长度0.8m，直径0.02m，质量1kg
为模拟能量损耗，滑动轴处有0.2的滑动摩擦系数和10的阻尼，单位为webots默认单位。

2、控制系统

控制目标为实现单腿跳跃机器人稳定跳跃运动，能跳台阶、越障以及在外力干扰下恢复稳定。

单腿跳跃机器人控制系统将跳跃、前进速度和身体姿态作为三个独立的控制问题。

跳跃高度控制（Hopping Heigh）

弹簧倒立摆模型将腿部模拟成一个弹簧，本文采用阻抗控制的方法，通过检测虚拟弹簧的压缩量，来计算滑动关节上直线电机的力：

${{\rm{F}}_s} = {k_s}\left( {{r_0} - r} \right)$

上式中， ${r_0}$ 和 $r$ 分别为弹簧原长和弹簧实际长度， ${k_s}$ 是弹簧刚度，弹簧刚度决定了机器人弹跳的频率。

跳跃机器人有节奏的上下跳动，在弹簧腿的作用下，可以认为是机器人物理系统的势能和动能在能量层面的相互转换。在能量转换过程中，必然因阻尼、摩擦、碰撞等因素导致能量损失，结果系统总能量越来越少，弹跳高度越来越低，最终系统失稳。

因此，必须在弹簧腿的伸长阶段（THRUST相），为机身额外提供一个推力 ${{\rm{F}}_{thrust}}$ 从而补充能量损失：

${{\rm{F}}_s} = {k_s}\left( {{r_0} - r} \right) + {F_{thrust}}$

理论上，可以通过下式来计算推力：

推力=（达到所需跳跃高度所需的能量-当前系统总能量）/ 弹簧伸长量

或者下式：

推力=每个周期系统能量损耗 / 弹簧伸长量

但这里使用了一种更简单的方法。直观理解，系统的能量损失应该是随着跳跃高度的增加而增加的，因此能量损失与跳跃高度是单调函数关系，所以对于每个固定的推力大小存在唯一的稳定跳跃高度，并且推力越大跳跃高度越大。实际上，推力和跳跃高度之间的关系非常复杂，无法直接理论求解。Marc Raibert采用了实验标定的方法，然后从中挑选一个比较稳定的推力大小。

如上图，图中thrust为推力，z为高度。最上面曲线是质心高度，中间是足底高度，最下面曲线是推力大小。每隔5s改变一次推力的大小，可见机身高度与推力成正相关关系。

水平速度控制（Control Forward Speed）

核心思想：足底在飞行相结束时第一次接触地面时的位置（落地点）决定了随后支撑相的加速度。作者提出了三个概念：

水平速度（Forward Speed）：机身水平方向的前进速度。

净加速度（net forward acceleration）：支撑相开始到支撑相结束这段时间机身水平速度增量。注意：在飞行相，忽略空气阻力，仅受重力时，机身水平速度不会改变。

中性点（neutral point）：对于每个水平速度，都有一个唯一的落地点，在此落地点上净加速度为0。下图中 ${x_f}$ 为落地点， $\dot x$ 为水平速度， $\Delta \dot x$ 为净加速度。

（a）：当落地点在落在比中性点远的位置时，一部分动能转化为势能，系统在支撑相阶段减速，净加速度>0。（b:）当落地点在落在中性点时，一部分动能转化为势能，净加速度=0。（c）: 当落地点在落在比中性点近的位置时，一部分势能转化为动能，系统在支撑相阶段加速，净加速度<0

因此，落地点决定了支撑相的净加速度，通过净加速度可以调整水平速度。

计算落地点

落足点的计算包含两个因素：一个是通过估计水平速度，来估计中性点的位置；第二个是水平速度的误差用于计算净加速度，从而计算为了实现这个净加速度，落足点相对于中性点的偏移量。两者直接相加，得到落足点。

首先计算中性点：

${{\bf{x}}_{f0}} = \frac{{{\bf{\dot x}}{T_s}}}{2}$

上式中， ${\bf{\dot x}}$ 为水平速度的状态估计，其算法在后续小节中详细介绍。 ${T_s}$ 为上一个支撑相的持续时间，即足底传感器触地到足底传感器离地的时间。

为了使机器变速，控制系统引入了不对称性。需要加速来稳定水平速度，以防止误差和外部干扰，并从一个水平速度改变到另一个水平速度。为了主动地给机器人变速，控制系统主动在落地点和中性点之间增加一个偏移量，使用水平速度误差的线性函数来确定该偏移量：

${{\bf{x}}_{f\Delta }} = {{\bf{k}}_{\dot x}}\left( {{\bf{\dot x}} - {{{\bf{\dot x}}}_d}} \right)$

其中， ${\bf{\dot x}}$ 是水平速度的状态估计， ${{\bf{k}}_{\dot x}}$ 是可调的反馈增益， ${{\bf{x}}_d}$ 是期望速度。本文中，含有下标d的符号，不加说明时都是指期望值（desire）。

结合上式可得：

${{\bf{x}}_f} = \frac{{{\bf{\dot x}}{T_s}}}{2} + {{\bf{k}}_{\dot x}}\left( {{\bf{\dot x}} - {{{\bf{\dot x}}}_d}} \right)$

注意，上式中落足点为{H}坐标系下落足点，具体定义见下一节。

控制落足点

定义三个坐标系{W}、{H}和{B}。坐标系{W}是在实验室固定的世界坐标系。坐标系{H}的原点随髋部移动，但其方向与{W}的方向保持平行。对于{W}和{H}，y与重力矢量对齐并向上为正。{B}固定在机身上，原点与{H}相同。参考webots惯性单元，可得旋转变换：

$\left\{ \begin{array}{l} {}_H^B{\bf{R}} = RotY\left( {yaw} \right)RotZ\left( {pitch} \right)RotX\left( {roll} \right)\\ {}_B^H{\bf{R}} = {}_H^B{{\bf{R}}^T}\\ {}^H{\bf{p}} = {}_H^B{\bf{R}}{}^B{\bf{p}}\\ {}^B{\bf{p}} = {}_B^H{\bf{R}}{}^H{\bf{p}} \end{array} \right.$

X轴电机和Z轴电机虽然在同一空间位置上，但其转动顺序对运动学解有影响。参考webots的hinge2jiont模型，可见Axis2（对应Z轴电机）在Parent节点上，Axis1（对应X轴电机）在End point节点上，因此逆运动学为：

$\left\{ \begin{array}{l} {\gamma _{xd}} = atan\left( {\frac{{ - z}}{{ - y}}} \right)\\ r = \sqrt {{x^2} + {y^2} + {z^2}} \\ {\gamma _{zd}} = asin\left( {\frac{x}{r}} \right) \end{array} \right.$

其中 $r$ 为杆长，可直接由距离传感器测得。 ${}^Bp = {\left[ {\begin{array}{*{20}{c}} x&y&z&1 \end{array}} \right]^T}$ {B}坐标系下足底坐标。

采用一个简单的线性伺服系统控制关节角度：

${\bf{\tau }} = - {{\bf{k}}_p}\left( {{\bf{\gamma }} - {{\bf{\gamma }}_d}} \right) - {{\bf{k}}_v}{\bf{\dot \gamma }}$

上式中， ${\bf{\tau }}$ 是关节电机的扭矩， ${\bf{\gamma }}{\bf{\dot \gamma }}$ 是传感器测得的关节位置和速度， ${{\bf{\gamma }}_{\bf{d}}}$ 是期望关节角。

${{\bf{k}}_{\bf{p}}}{{\bf{k}}_{\bf{v}}}$ 分别是伺服系统的弹簧和阻尼。

水平速度的状态估计

在飞行阶段，不考虑空气阻力，仅受垂直方向的重力，水平速度不变。因此水平速度的状态估计是在支撑相实现的，正运动学：

${}^B{\bf{p}} = \left[ {\begin{array}{*{20}{c}} {rsin{\gamma _z}}\\ { - rcos{\gamma _z}cos{\gamma _x}}\\ { - rcos{\gamma _z}sin{\gamma _x}}\\ 1 \end{array}} \right]$

转换到{H}坐标系下：

${}^H{\bf{p}} = {}_H^B{\bf{T}}{}^B{\bf{p}}$

假设足底不打滑，即足底点与世界坐标系刚性连接，则足底在{H}下的运动与{H}在{W}下的运动大小相等，方向相反。即：

${\bf{\dot x}} = \left[ {\begin{array}{*{20}{c}} { - {}^H{\bf{\dot p}}.x}\\ { - {}^H{\bf{\dot p}}.z} \end{array}} \right]$

然后，又随便加了个低通滤波器，反正是仿真环境，没什么干扰。

姿态控制（Control Body Attitude）

控制系统通过在支撑相对臀部关节施加扭矩来保持身体姿态。由于角动量在飞行过程中是守恒的，所以只有支撑相才能改变系统的角动量。在支撑相，脚和地面之间的摩擦允许向身体施加扭矩，而不会引起腿部较大的加速度。控制系统通过一个线性伺服来实现：

${\bf{\tau }} = - \left( { - {{\bf{k}}_p}\left( {{\bf{\theta }} - {{\bf{\theta }}_d}} \right) - {{\bf{k}}_v}{\bf{\dot \theta }}} \right)$

其中， ${\bf{\theta }} = \left[ {\begin{array}{*{20}{c}} {pitch}\\ {roll} \end{array}} \right]$ ,注意，尽管陀螺仪的正方向和关节电机传感器的正方向相同，但是关节电机的扭矩作用在腿部和作用在身体上的方向是相反的，因此这里多了一个负号。

状态机

跳跃机器人根据自身的运行阶段，分为五种状态，这五种状态名称、触发条件、以及在该状态下机器人执行的命令见下表：

来源：知乎 www.zhihu.com

作者：xyYu

【知乎日报】千万用户的选择，做朋友圈里的新鲜事分享大牛。
点击下载