四足机械狗_mit 四足代码分析

作者：繁依Fanyi0 | 2024-04-13 05:44:02

踩

mit 四足代码分析

机械狗

写在前面

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

计划

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

阶段	平台	开始时间
搭建模拟环境	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)

Two-link manipulator
为了方便起见，先就忽略转动惯量了。这个其实应该没什么大事，毕竟我要造的玩具狗腿摆得又不会很快。这里第一个关节代表腿根部，两杆子的末端（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
1
2
3
4
5
6
7
8
9

Matlab output: (直接复制symbolic 的latex code,然后最外面加四美元号）
$\left($

\begin{matrix} 2 L_{2} \cos (θ_{1} + θ_{2}) + 2 L_{1} \cos (θ_{1}) \\ 2 L_{2} \sin (θ_{1} + θ_{2}) + 2 L_{1} \sin (θ_{1}) \end{matrix}

$\begin{array}{c} 2\,L_2 \,\cos \left(\theta_1 +\theta_2 \right)+2\,L_1 \,\cos \left(\theta_1 \right)\\ 2\,L_2 \,\sin \left(\theta_1 +\theta_2 \right)+2\,L_1 \,\sin \left(\theta_1 \right) \end{array}$ \right)

(2 L_{2} cos (θ_{1} + θ_{2}) + 2 L_{1} cos (θ_{1}) 2 L_{2} sin (θ_{1} + θ_{2}) + 2 L_{1} sin (θ_{1}))

最后把这个解析解变成一个可以输入数值的方程：

fk_two_link_func = matlabFunction(Xee)
1

1.1.2 Inverse Kinematics

Inverse Kinematics 就是给EE的位置算出达到这个位置时关节需要的角度。注意，kinematics完全不考虑外力啊什么的。
算这个inverse kinematics 有两种算法：
一种是手推，具体推法可以参考这个视频：
手推IK
但这玩意有个问题，只能在第一象限用。本人对三角函数啥的一直没很彻底的悟。所以也不知道为啥只能在第一象限用。

第二种是直接用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)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

Output:
$\left($

\begin{matrix} 2 a t a n (\frac{4 L_{1} y t + \sqrt{- 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 (\frac{4 L_{1} y t - \sqrt{- 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 (\frac{\sqrt{(- 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 (\frac{\sqrt{(- 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}}) \end{matrix}

$\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)

⎝ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎛ 2 a t a n (\frac{4 L _{1} y t + - 1 6 L _{1} ^{4} + 3 2 L _{1} ^{2} L _{2} ^{2} + 8 L _{1} ^{2} x t ^{2} + 8 L _{1} ^{2} y t ^{2} - 1 6 L _{2} ^{4} + 8 L _{2} ^{2} x t ^{2} + 8 L}{4 L _{1} ^{2} + 4 L _{1} x t - 4 L _{2} ^{2} + x t ^{2} + y t ^{2}}

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/繁依Fanyi0/article/detail/414712