四连杆机构运动学推导

最新推荐文章于 2024-12-07 14:57:28 发布

林三程

最新推荐文章于 2024-12-07 14:57:28 发布

阅读量3.4k

点赞数 1

文章标签：算法机器人

本文链接：https://blog.csdn.net/aniclever/article/details/134078080

版权

简单推了一下，然后发现没啥必要推导。毕竟花了时间，写下来聊以记录。

四连杆机构简图

本文是交叉的四连杆推导，不交叉的也是一样的，没啥区别。

为方便记录，将 $cos(\theta_i)$ 记为 $c_i$ ，其中， $\theta_1$ 为驱动角度，在以下推导中是已知量

方法一：利用矢量闭环计算 $\theta_2$ 和 $\theta_3$

从A出发，两个方向的矢量和同时到达C点：

$\vec{l_0}+\vec{l_3}=\vec{l_1}+\vec{l_2}$

等式两边的xy分量相等，有：

$\left\{\begin{matrix} l_1c_1+l_2c_2 &= & -l_0+l_3c_3\\ l_1s_1+l_2s_2& = & l_3s_3 \end{matrix}\right.$

整理一下有：

$\left\{\begin{matrix} l_2c_2-l_3c_3 &= & -l_0-l_1c_1\\ l_2s_2-l_3s_3& = &- l_1s_1 \end{matrix}\right.$

可以使用matlab解这个方程组：

% 首先数据初始化
l0 = 5;
l1 = 10;
l2 = 5;
l3 = 10;
syms th2 th3
% 解方程
th1 = pi/3*2;
[th2, th3] = vpasolve(l2*cos(th2)-l3*cos(th3)==-l0-l1*cos(th1),...
                    l2*sin(th2)-l3*sin(th3)==-l1*sin(th1),th2,th3);

这个式子可以写成矩阵形式，并且可以进一步求导从而得到角速度之间的关系。

方法二：利用连杆长度不变计算 $\theta_2$

表示出D点和C点后，利用DC连杆的长度不变，可以列出等式

$\begin{cases} & C:(l_1c_1+l_2c_2,l_1s_1+l_2s_2) \\ & D:(-l_0,0) \end{cases}$

由 $|CD|^2=l_3^2$ ，可得：

$(l_1c_1+l_2c_2+l_0)^2+(l_1s_1+l_2s_2)^2=l_3^2$

展开，并化简：

$2(l_1c_1+l_0)l_2c_2+2l_1s_1l_2s_2=l_3^2-(l_1c_1+l_0)^2-l_2^2-l_1^2s_1^2$

等式中，只有 $\theta _2$ 是未知的，其他的都是已知的，为方便计算，将已知量做以下定义：

$\begin{cases} E = 2(l_1c_1+l_0)l_2\\ F = 2l_1s_1l_2\\ G = l_3^2-l_2^2-l_1^2s_1^2-(l_1c_1+l_0)^2 \end{cases}$

则等式变为： $Ec_2+Fs_2=G$ ，这里可以用matlab解方程得到结果，或者进一步推一下

以下计算都会涉及多解，因此sin和cos都写出来才能确定解

设：

$\alpha=arcsin(\frac{E}{\sqrt{E^2+F^2}}), \alpha=arccos(\frac{F}{\sqrt{E^2+F^2}})$

$\beta=arccos(\frac{E}{\sqrt{E^2+F^2}}), \beta=arcsin(\frac{F}{\sqrt{E^2+F^2}})$

则有： $\begin{cases} sin(\theta_2+\alpha) = \frac{G}{\sqrt{E^2+F^2}} \\ cos(\theta_2-\beta) = \frac{G}{\sqrt{E^2+F^2}} \end{cases}$

根据反三角函数可以计算出 $\theta_2$ :

$\begin{cases} \theta_2 = arcsin(\frac{G}{\sqrt{E^2+F^2}})-\alpha \\ \theta_2 = arccos(\frac{G}{\sqrt{E^2+F^2}})+\beta \end{cases}$

四连杆机构运动学推导

方法一：利用矢量闭环计算和

方法二：利用连杆长度不变计算

方法一：利用矢量闭环计算 $\theta_2$ 和 $\theta_3$

方法二：利用连杆长度不变计算 $\theta_2$