五连杆正运动学解算

正运动学解算

如图所示五连杆模型，其中、为关节电机；、为电机输出角度，可由编码器读出；为连杆长度。需要求解和。

以中点为原点，为轴，垂直的向上为轴，建立平面直角坐标系。设的坐标分别为。

对于点，有：

对公式移项并平方相加，得：

令，则有：

移项，令，则有：

由二倍角公式，有：

则式可化为：

整理得：

式为关于的一元二次方程，其判别式：

其解为：

则有：

考虑机械结构及限位，，当时，判别式恒成立：

易知式的解其实就是和。根据图中关系有：

因此取正根：

由此可求出点坐标：

化为极坐标系，解出和：

至此，五连杆正运动学解算完毕。

计算力矩转换的雅可比矩阵

上面由正运动学解算将五连杆机构构造成了一个虚拟的倒立摆模型。虽然可以简化我们的控制算法，但在实际控制中，仍然需要将电机的力矩转换为虚拟倒立摆的力矩。我们可以通过雅可比矩阵来实现这个转换。

本部分参考玺佬的文章，这里只贴 matlab 代码，具体推导过程请参考原文。

syms x_A x_E y_A y_E phi_1(t) phi_2(t) phi_3(t) phi_4(t) phi_dot_1 phi_dot_4 l_1 l_2 l_3 l_4 l_5

% 位置关系
x_A = -l_5/2;
x_E = l_5/2;
y_A = 0;
y_E = 0;

x_B = x_A + l_1*cos(phi_1);
y_B = y_A + l_1*sin(phi_1);
x_D = x_E + l_4*cos(phi_4);
y_D = y_E + l_4*sin(phi_4);

x_C = x_B + l_2*cos(phi_2);
y_C = y_B + l_2*sin(phi_2);
% 速度关系
x_dot_B = diff(x_B,t);
y_dot_B = diff(y_B,t);
x_dot_D = diff(x_D,t);
y_dot_D = diff(y_D,t);

x_dot_C = diff(x_C,t);
y_dot_C = diff(y_C,t);

phi_dot_2 = ((x_dot_D-x_dot_B)*cos(phi_3)+(y_dot_D-y_dot_B)*sin(phi_3))/l_2/sin(phi_3-phi_2);
% 代数
x_dot_C = subs(x_dot_C,diff(phi_2,t),phi_dot_2);
x_dot_C = subs(x_dot_C,[diff(phi_1,t),diff(phi_4,t)],[phi_dot_1,phi_dot_4]);
y_dot_C = subs(y_dot_C,diff(phi_2,t),phi_dot_2);
y_dot_C = subs(y_dot_C,[diff(phi_2,t),diff(phi_1,t),diff(phi_4,t)],[phi_dot_2,phi_dot_1,phi_dot_4]);
% 计算雅可比矩阵
x_dot = [x_dot_C;y_dot_C];
q_dot = [phi_dot_1;phi_dot_4];
x_dot = simplify(collect(x_dot,q_dot));
J=simplify(jacobian(x_dot,q_dot))
% 转极坐标
syms phi_0 l_0

R = [cos(phi_0-pi/2) -sin(phi_0-pi/2);
     sin(phi_0-pi/2)  cos(phi_0-pi/2)];
M = [0 -1/l_0;
     1     0];
T = simplify(J.'*R*M)

最后得出的结果与原文一致：