三自由度机械臂连续轨迹控制Simulink仿真

博客搬家到自己搭建的 主页(wonghaotian.com) 啦q(≧▽≦q)，大家快来逛逛鸭！

 

提供完整文件下载（Click to SAVE）

DEMO

 

控制要求

机械臂初始时刻姿态自行确定，经过5s机械臂末端执行器沿直线运动到目标点x=0.3m、y=0.5m、z=1.2m，5s之后机械臂停留在目标点。沿直线运动属于连续轨迹控制，不能直接指定起始点和终止点进行点对点控制。

 

结构参数

机械臂结构参数

 

分析流程

1. 在Solidworks中建立真实机械臂模型，针对MATLAB修改模型，把电机等和相连杆件合并成一个刚体，导出到SimMechanics中，检查坐标系和初始位置是否正确；
2. 修改转动副参数，添加信号输入、输出端口，连接PID控制回路；
3. 插入示波器测量末端x、y、z三个方向上的位移，插入XY、YZ、XZ三个平面内的轨迹显示器；
4. 编写位置反解函数；
5. 先输入（x,y,z）坐标进行点位控制的调试，确认位置反解结果是否正确；
6. 确定无误后，插入x、y、z与时钟变量u的函数关系；
7. 调试PID参数，以获得更平稳、更接近直线轨迹的运动。

Simulink模型

 

 

 

位置反解程序

MATLAB在符号计算方面很弱，关于运动学逆解公式是怎么得来的，这里有一条经验：三个自由度用 MATLAB 直接求解是解不出来的，我们注意到q1直接由x、y可以得出，跟其他的没什么关系，这样把原本的3个方程3个未知数转化成2个方程2个未知数，MATLAB轻松求解。当然，Mathematica可以直接求解三元符号方程组。

function [q1,q2,q3] = fcn(x,y,z)
% Iuput: 目标点坐标x,y,z;
% Output: 各关节转角q1,q2,q3;
%% 结构参数
L1 = 0.3;    % 转动副2回转中心到地面的垂直距离
L2 = 0.6;    % 转动副2、3之间的距离
L3 = 0.6;    % 转动副3回转中心到末端探头球心的距离
%% 运动学逆解
q1 = -atan(x/y);
q2 = -2*atan((2*L2*(L1 - z) + (L2^2*((x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3)*(L2^2 + L3^2 - x^2 - y^2 - (L1 - z)^2 + 2*L2*L3))^(1/2))/(x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3) + (L3^2*((x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3)*(L2^2 + L3^2 - x^2 - y^2 - (L1 - z)^2 + 2*L2*L3))^(1/2))/(x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3) - ((L1 - z)^2*((x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3)*(L2^2 + L3^2 - x^2 - y^2 - (L1 - z)^2 + 2*L2*L3))^(1/2))/(x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3) - ((x^2 + y^2)*((x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3)*(L2^2 + L3^2 - x^2 - y^2 - (L1 - z)^2 + 2*L2*L3))^(1/2))/(x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3) - (2*L2*L3*((x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3)*(L2^2 + L3^2 - x^2 - y^2 - (L1 - z)^2 + 2*L2*L3))^(1/2))/(x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3))/(2*L2*(x^2 + y^2)^(1/2) + L2^2 - L3^2 + x^2 + y^2 + (L1 - z)^2));
q3 = -2*atan(((x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3)*(L2^2 + L3^2 - x^2 - y^2 - (L1 - z)^2 + 2*L2*L3))^(1/2)/(x^2 - L3^2 - L2^2 + y^2 + (L1 - z)^2 + 2*L2*L3));
end

这里是空间3自由度，q1比较明显，在平面3自由度中也是完全一样的，一定有一个自由度可以被转化掉，但是不那么明显。在平面三自由度位置反解中，你会发现只能列出2个约束方程，可是要解出3个未知量，所以必须再加一个方程，也就是末端姿态角theta，要求机械臂不仅运动到指定点，而且满足末端执行器角度要求。这里我也把平面三自由度的位置反解程序给出来吧，大家自己体会。

% 平面三自由度位置反解程序

% 直接列3个方程企图求解3个未知量
% 大家可以试一下，用MATLAB解这个符号方程组会让你怀疑人生，根本解不出来
syms L1 L2 L3 q1 q2 q3 x y theta
eqn1 = x == L1 * cos(q1) + L2 * cos(q1 + q2) + L3 * cos(theta);
eqn2 = y == L1 * sin(q1) + L2 * sin(q1 + q2) + L3 * sin(theta);
eqn3 = theta == q1 + q2 + q3;
[q1, q2, q3] = solve(eqn1, eqn2, eqn3, q1, q2, q3)


% 我们对上述程序改动一下，干掉3个方程
% 既然 theta=q1+q2+q3 我们考虑只求q1、q2，之后q3自然出来了
syms L1 L2 L3 q1 q2 q3 x y theta
syms u v
eqn1 = L1 * cos(q1) + L2 * cos(q1 + q2) == u;  % 其中 u = x-L3*cos(theta)
eqn2 = L1 * sin(q1) + L2 * sin(q1 + q2) == v;  % 其中 v = y-L3*sin(theta)
[q1, q2] = solve(eqn1, eqn2, q1, q2)
q3 = theta - q1 - q2