四足机器人质心准静态控制器（三）

四足机器人质心准静态控制器（一）

四足机器人质心准静态控制器（二）

参考工程代码 GitHub

逆向运动学

工程所给出的足式机器人是经典的单腿三自由的度末端点接触结构。那我们在规划末端状态的时候就可以不用考虑末端姿态，只要考虑末端点位置就可以了。三自由度刚好可以末端位置在任务空间种的要求。所谓逆向运动学就是在任务空间的位置和姿态映射到关节空间。一般的多自由度关节机器人进行逆向运动学规划，需要进行违逆迭代求解逆向运动学，有一定的运算量。当然对于这种三自由度的串级关节腿部，有也可以利用余弦定理和三角恒等式来进行求解。但是这样子的求解一般都针对了特定的膝关节朝向。工程demo中所使用的是违逆迭代的数值求解方法，接下来看详细的推导过程。

以一个腿为例，由于我们只要求三轴位移空间，且只有三个自由度，对应的雅可比矩阵是 3X3的。可以得到一个关节角速度到任务空间速度的映射。以离散的角度来看，在任务空间中，期望位置和当前位置有很小的误差，需要在关节空间求解一个很小的关节增量。以最小化角度增量来看，服从离散的正向关系，通过拉格朗日乘子法求解可以得到最终的右伪逆矩阵。需要注意的是，这中解法适合当前这种三自由度的腿部系统，如果是大于3个自由度，那么将不适用。该式对角度规划没有进一步约束，有可能会导致多个角度状态到达边界，没办法正常跟踪轨迹。

Minimum Jerk轨迹

以左式为目标，通过欧拉拉格朗日方程求解得到 L 的解，由于L的解非常简单，所以进行多重积分后可以得到一个很有规律的式子，就是时域的五次多项式轨迹，而且只要确定6个系数即可。

我们可以把这个6个待求解系数组成一个向量Q。这里先讨论两个点，由x1点到x2点可以分别指定位置速度和加速度由于求解6次样条曲线都需要满足这两个点，并且是从 t1 到 t2 ，那么可以对应X12向量可以得到一个 6X6的矩阵。解说前三行，可以发现第二行是对于 6次多项式求一阶导数，第三行是求得二阶导数。 X12和6X6方阵都是已知的，那么只要对方阵求逆就可以得到向量Q。如此以来，6次多项式就可以被确定，轨迹描述为在t1时从x1出发，在t2时刻到达x2点，且端点的速度和加速度都被确定。

那么，现在就自然诞生一个问题，多点和多维度的轨迹如何解决。多点轨迹其实非常同意解决，把一段段轨迹接起来，对每段轨迹都得到一个 Q，在不同时间段计算不同的轨迹时间函数即可。多维度轨迹也可以用很简单的思想，比如，对于XYZ轴每一个维度分别计算样条函数。计算完毕后再拼成一个向量。这样子的过程，在工程demo中已经专门写成了一个文件，可以很方便进行调用。

Minimum jerk 轨迹在我的工程demo中其实是统一的方法。不仅仅用在质心轨迹规划，也包括了摆对腿部轨迹规划部分。对于质心轨迹规划，XYZ轴位移分了三个维度处理。对于姿态部分只用了一个维度来处理，且范围是0到1，其实就是百分比的意思。期望姿态可以观察右式，可以发现先从李群映射到向量空间得到误差向量，再乘以百分比后指数映射到李群，考虑初始的状态，再对数映射到向量空间后可以得到一个当前时间下的rpy向量。摆动腿轨迹，在实际环境中是要考虑地形和障碍的。此处仅限于平地进行讨论。摆动腿的轨迹，有很多种。比如有使用摆线轨迹的，使用二次函数的，使用B样条的，有使用三次样条的。但就目前主要考虑的摆动轨迹的作用主要是可以跨越障碍，因此不考虑步态和整体控制方法，其使用什么样的轨迹并不是很重要。在工程中同样使用minimum jerk 轨迹规划摆动轨迹。那么只要定义三个点就可以确定。但是由于无法感知地形，还有一些其他干扰因素，导致理想的最终点可能不会完全接触地面。所以要额外的补上一节。摆动腿运动的截至状态就是末端被认为是接触了地面。这一点在工程通过touch sensor 来判断。所以一段摆动轨迹规划，可以有四个点。当然也可以有五个点，五个点更加接近于一个方形。因为，圆弧轨迹在跨越障碍时，可能末端在未到达最高点时就会碰撞。为了简化这部分逻辑可以先将就使用五点方形轨迹。