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

要求：

机械臂初始时刻姿态自行确定，经过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模型

 

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