四足机械狗

机械狗

---

---

写在前面

可能要写很长很久，可能一年吧

---

计划

目前我打算先做模拟，然后在模拟环境中设计控制器，全部调得很完美后再开始搭。平台选择是这样的：

阶段	平台	开始时间
搭建模拟环境	matlab + simscape body	07/2021
控制器设计	matlab + simulink	07/2021
实机代码	C++ in ROS	10/2021??
造狗	3D 打印	2022？

目前对狗的器官是这么设想的：

部位	选材
大脑 (上位机）	Navida Jeston Nano
小脑 （下位机）	STM32 F7+
关节	先用舵机，能动了再换电机
身体	先3D打印，可以了再CNC+表面处理

---

1 搭建模拟环境

1.1先模拟条简单的狗腿 （two-link)

为了方便起见，先就忽略转动惯量了。这个其实应该没什么大事，毕竟我要造的玩具狗腿摆得又不会很快。这里第一个关节代表腿根部，两杆子的末端（end-effector,EE)是狗脚。

1.1.1 Forward Kinematics

这边我用matlab里symbolic math 来推导所有的kinematics.
这里x2 y2 是 EE 的位置。

sympref('AbbreviateOutput', false); 
% prevent symbolic abbreviations
syms L1 L2 theta1 theta2
x1 = 2*L1*cos(theta1);
y1 = 2*L1*sin(theta1);
x2 = x1 + 2*L2*cos(theta1+theta2);
y2 = y1 + 2*L2*sin(theta1+theta2);
Xee= [x2;
      y2] %end effector position

Matlab output: (直接复制symbolic 的latex code,然后最外面加四美元号）
$$\left(\begin{array}{c}2L_2\cos \left({\theta }_1+{\theta }_2\right)+2L_1\cos \left({\theta }_1\right)\\ 2L_2\sin \left({\theta }_1+{\theta }_2\right)+2L_1\sin \left({\theta }_1\right)\end{array}\right)$$
最后把这个解析解变成一个可以输入数值的方程：

fk_two_link_func = matlabFunction(Xee)

---

1.1.2 Inverse Kinematics

Inverse Kinematics 就是给EE的位置算出达到这个位置时关节需要的角度。注意，kinematics完全不考虑外力啊什么的。
算这个inverse kinematics 有两种算法：
一种是手推，具体推法可以参考这个视频：

但这玩意有个问题，只能在第一象限用。本人对三角函数啥的一直没很彻底的 悟 。 所以也不知道为啥只能在第一象限用。

第二种是直接用symbolic math 自动推。好处是四个象限都能用，而且不可能出错。最重要的它能给出所有的解：选择适合的解能帮助我们实现狗腿是向前弯还是向后弯。

第三种是用matlab里的robotic toolbox，但我现在还没试过。

第二种方法代码如下，xt yt 是目标点的坐标：

sympref('AbbreviateOutput', false); 
% prevent symbolic abbreviations
syms L1 L2 theta1 theta2
x1 = 2*L1*cos(theta1);
y1 = 2*L1*sin(theta1);
x2 = x1 + 2*L2*cos(theta1+theta2);
y2 = y1 + 2*L2*sin(theta1+theta2);
Xee= [x2;
      y2]; %end effector position
      
syms xt yt
eq_ik_twolink_RHS = [xt;
                     yt];
Theta_twolink = [theta1;
                 theta2];
S_ik_twolink = solve(Xee==eq_ik_twolink_RHS,Theta_twolink);
ik_two_link = [S_ik_twolink.theta1;
               S_ik_twolink.theta2];
ik_two_link = simplify(ik_two_link)

Output:
( 2   a t a n ( 4   L 1   y t + − 16   L 1 4 + 32   L 1 2   L 2 2 + 8   L 1 2   x t 2 + 8   L 1 2   y t 2 − 16   L 2 4 + 8   L 2 2   x t 2 + 8   L 2 2   y t 2 − x t 4 − 2   x t 2   y t 2 − y t 4 4   L 1 2 + 4   L 1   x t − 4   L 2 2 + x t 2 + y t 2 ) 2   a t a n ( 4   L 1   y t − − 16   L 1 4 + 32   L 1 2   L 2 2 + 8   L 1 2   x t 2 + 8   L 1 2   y t 2 − 16   L 2 4 + 8   L 2 2   x t 2 + 8   L 2 2   y t 2 − x t 4 − 2   x t 2   y t 2 − y t 4 4   L 1 2 + 4   L 1   x t − 4   L 2 2 + x t 2 + y t 2 ) − 2   a t a n ( ( − 4   L 1 2 + 8   L 1   L 2 − 4   L 2 2 + x t 2 + y t 2 )   ( 4   L 1 2 + 8   L 1   L 2 + 4   L 2 2 − x t 2 − y t 2 ) − 4   L 1 2 + 8   L 1   L 2 − 4   L 2 2 + x t 2 + y t 2 ) 2   a t a n ( ( − 4   L 1 2 + 8   L 1   L 2 − 4   L 2 2 + x t 2 + y t 2 )   ( 4   L 1 2 + 8   L 1   L 2 + 4   L 2 2 − x t 2 − y t 2 ) − 4   L 1 2 + 8   L 1   L 2 − 4   L 2 2 + x t 2 + y t 2 ) ) \left(\begin{array}{c} 2\,\mathrm{atan}\left(\frac{4\,L_1 \,\mathrm{yt}+\sqrt{-16\,{L_1 }^4 +32\,{L_1 }^2 \,{L_2 }^2 +8\,{L_1 }^2 \,{\mathrm{xt}}^2 +8\,{L_1 }^2 \,{\mathrm{yt}}^2 -16\,{L_2 }^4 +8\,{L_2 }^2 \,{\mathrm{xt}}^2 +8\,{L_2 }^2 \,{\mathrm{yt}}^2 -{\mathrm{xt}}^4 -2\,{\mathrm{xt}}^2 \,{\mathrm{yt}}^2 -{\mathrm{yt}}^4 }}{4\,{L_1 }^2 +4\,L_1 \,\mathrm{xt}-4\,{L_2 }^2 +{\mathrm{xt}}^2 +{\mathrm{yt}}^2 }\right)\\ 2\,\mathrm{atan}\left(\frac{4\,L_1 \,\mathrm{yt}-\sqrt{-16\,{L_1 }^4 +32\,{L_1 }^2 \,{L_2 }^2 +8\,{L_1 }^2 \,{\mathrm{xt}}^2 +8\,{L_1 }^2 \,{\mathrm{yt}}^2 -16\,{L_2 }^4 +8\,{L_2 }^2 \,{\mathrm{xt}}^2 +8\,{L_2 }^2 \,{\mathrm{yt}}^2 -{\mathrm{xt}}^4 -2\,{\mathrm{xt}}^2 \,{\mathrm{yt}}^2 -{\mathrm{yt}}^4 }}{4\,{L_1 }^2 +4\,L_1 \,\mathrm{xt}-4\,{L_2 }^2 +{\mathrm{xt}}^2 +{\mathrm{yt}}^2 }\right)\\ -2\,\mathrm{atan}\left(\frac{\sqrt{ {\left(-4\,{L_1 }^2 +8\,L_1 \,L_2 -4\,{L_2 }^2 +{\mathrm{xt}}^2 +{\mathrm{yt}}^2 \right)}\,{\left(4\,{L_1 }^2 +8\,L_1 \,L_2 +4\,{L_2 }^2 -{\mathrm{xt}}^2 -{\mathrm{yt}}^2 \right)}}}{-4\,{L_1 }^2 +8\,L_1 \,L_2 -4\,{L_2 }^2 +{\mathrm{xt}}^2 +{\mathrm{yt}}^2 }\right)\\ 2\,\mathrm{atan}\left(\frac{\sqrt{ {\left(-4\,{L_1 }^2 +8\,L_1 \,L_2 -4\,{L_2 }^2 +{\mathrm{xt}}^2 +{\mathrm{yt}}^2 \right)}\,{\left(4\,{L_1 }^2 +8\,L_1 \,L_2 +4\,{L_2 }^2 -{\mathrm{xt}}^2 -{\mathrm{yt}}^2 \right)}}}{-4\,{L_1 }^2 +8\,L_1 \,L_2 -4\,{L_2 }^2 +{\mathrm{xt}}^2 +{\mathrm{yt}}^2 }\right) \end{array}\right) ⎝⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎛​2atan(4L1​2+4L1​xt−4L2​2+xt2+yt24L1​yt+−16L1​4+32L1​2L2​2+8L1​2xt2+8L1​2yt2−16L2​4+8L2​2xt2+8L2​2yt2−xt