imuncle
commented
4 years ago Open imuncle opened 4 years ago
imuncle
commented
4 years ago imuncle
commented
4 years ago 看了机器人学后颇有感触，有空把矩阵用在四足上
zhuangfengzi
commented
4 years ago 很棒！ 很期待！
FENYUN323
commented
4 years ago 太强了
距离上一篇文章已经有一个半月了，越咕越得劲（其实是因为小学期整天上实验课啦），前天刚结束小学期的课程，觉得是时候给这个小项目结个尾了。
我花了四个小时设计机械结构，十二个小时接线，三个小时写代码，九个小时改错调试，最后的作品长这样：
（
这TM也太丑了吧）因为能力有限，机械结构的刚度也不够，前面文章所设想的部分功能未能实现，深表遗憾。
骚话不多说，直接进正题。
腿部运动
我使用的是飞特的SCS0009舵机，体积很小，扭矩足够，串行总线，简直不要太爽！
因为舵机个体以及舵机安装方式的问题，同一个位置信号发过去，不同的舵机反应不一样，所以最开始我们需要校准。
我记录的是四条腿都伸直的时候的舵机位置反馈值，存储在一个数组里面：
当然小狗是不能保持伸直的，所以我在初始化的时候给了一个足尖初始坐标(0,0,86.6)，坐标系的建立见后面的图。
从前面的文章（四足机器人制作（一） 腿部运动算法）可以看出，我的腿部控制逻辑为：先确定足尖的坐标，然后根据几何关系求解出腿部三个舵机的角度，最后转换为对应的舵机位置信号。先上代码：
这里写的有点不规范，我为了图方便直接把我的狗腿长度带入计算公式化简了。我的小狗大腿和小腿长度都是50mm。
模型如下（注意坐标系的原点和坐标轴正方向）：
其中AF和FD长度都是50mm，可以通过解三角形得到，α（对应代码中angle_2），β（angle_3），γ（angle_1）的计算公式如下：
$$\gamma = arc\tan(\frac{BC}{CD})$$ $$\alpha = arc\cos(\frac{AD^2+AF^2-DF^2}{2AD·AF}) - arc\sin(\frac{DE}{AD})$$ $$\beta = 180 - arc\cos(\frac{AF^2+FD^2-AD^2}{2AF·FD})$$
把坐标和腿长带进去，就得到上面的代码。
上面的代码中，
delta_x之类的变量记录的是机器人各种变化（比如走路）对单条腿产生的坐标变化值，将它叠加在腿的初始位置上就得到了这个时刻腿的目标位置。根据这个位置计算完角度后，将角度换算为舵机的位置信息（最后三行），这里有变量coefficient_1和coefficient_2，这是因为在机械结构上，不同的舵机安装方式不同，对于“角度增大”这一指令，它既可能逆时针旋转，也可能顺时针旋转，所以在初始化的时候为每条腿设置了对应的系数（1或-1），拿右前腿为例，它的初始化代码如下：其实从上面的信息中还可以看出，86.6f正好是50的根号3倍，所以我的狗腿初始位置如下：
姿态解析
这里涉及到的姿态解析和之前的文章里讲的有些不同，因为坐标系不一样，先看yaw轴吧。
yaw
yaw轴和前述的情况一样，具体公式及推导见四足机器人制作（二） 姿态解析，实现代码如下：
顺便一提，我的小狗长为125.4mm，宽为113.9mm，上面出现的数值都是我带入公式之后化简后的。
pitch
pitch轴这里和之前的文章里写的不同，先看抽象一下：
这幅图有点小复杂，最关键的地方在于图中的那个小红色三角形，显然这是等腰三角形，顶角α就是机器狗的pitch轴角度，然后根据机械结构可以算出三角形的腰长L，以及图中的θ角，于是在红色三角形内通过解三角形就可以求出β角，底边长度，进而求出坐标的变化量。最后的公式如下： $$l = \sqrt{2L^2(1-\cos\alpha)}$$ $$\beta = \theta - \frac{\alpha}{2}$$ $$\delta x = -l\cos(\beta)$$ $$\delta z = l\sin(\beta)$$ 代码如下：
roll
roll轴和pitch轴的分析方法一模一样，就不再展开讲了，代码如下：
最后把三个轴整合起来：
有了姿态，那IMU平衡简直轻而易举，代码如下：
效果如下：
（
好像效果不是很明显）全向移动
在之前的理论分析中，我还以为我能实现walk（走）、pace（踱步）和trot（小跑），没想到只能实现速度最慢的walk。先回顾一下walk是什么样子：
仔细分析可以发现，四条腿是等周期间隔依次落地和抬起的，顺序可以是：右前→左后→左前→右后。假设间隔周期为T，那么对于一条腿来说，抬腿往前迈的动作需要在T内完成，然后保持放在地上的状态3T时间，然后循环。
考虑到前进、横移、转弯三种命令可能同时执行，也方便代码的书写，我把抬腿这个动作单独用一个函数计算，代码如下：
函数写的很
啰嗦（算了就这样吧），上面的函数每5ms执行一次，所以周期间隔T为180ms。在函数中四条腿按照设定的顺序依次抬起放下。剩下的三种运动用另一个函数表示，也就是说
delta_z由一个函数控制，delta_x和delta_y由另一个函数控制。运动函数代码如下：
为展示得更直观，只看前进的部分吧：
从上面的代码中可以看出，在腿抬起阶段（Up），腿从
-StepLength匀速移动到StepLength，在放下阶段（Down），又从StepLength匀速移动到-StepLength，实现了前进移动。横移和转弯也是类似，不做赘述。
云台稳定
在我之前的设想中，这一部分是整个机器人最复杂的部分，也是整个机器人最亮眼的地方，然而经过一番缜密的推算后发现，我之前提出的想法（惯导实现室内定位部分）无法实现，原因是传感器数据不够，本来想着在狗头上加一个光流传感器算了，但是千不该万不该我当初买舵机少买了一个，导致光流没地方装（现在就是后悔，非常后悔.jpg）
但最基本的机器狗原地稳定狗头还是能做的嘛。
我们需要解决两个轴：yaw和pitch，roll轴由于机械方面的限制而无法实现。我们一个一个来。
yaw
yaw轴的模型如下：
这里的计算和前面的pitch轴可以说是非常相似了，我们根据机械结构可以知道L的长度，可以算出β的值，α是机器狗的yaw轴偏转角度，很容易得出以下公式： $$l=\sqrt{2L^2(1-\cos\alpha)}$$ $$\beta = 90 - \frac{\alpha}{2}$$ $$\delta x = -l\cos\beta$$ $$\delta y = -l\sin\beta$$
实现代码如下：
pitch
不多说，建模：
和前面简直一模一样，直接上公式： $$l=\sqrt{2L^2(1-\cos\alpha)}$$ $$\beta = 90 - \frac{\alpha}{2}$$ $$\delta x = -l\cos\beta$$ $$\delta z = l\sin\beta$$
实现代码如下：
写在最后
回看发现，四足的底层控制其实不难，耐心分析就行。我的四足之旅到此就暂时停下了，后面或许还有机会再拾起它，代码整理之后也会放在GitHub上供大家参考，希望对四足机器人爱好者有所帮助。
最后简单列一下我的开发环境：