ros 机械臂复位_五自由度机械臂建模（matlab C）

五自由度机械臂建模

学习代码都记录在个人github上，欢迎关注~

Matlab机器人工具箱版本9.10

机械臂还是原来的机械臂，之前用ROS做底层驱动，不需要写正逆运动学和相关算法就能得到一些简单的仿真轨迹，详情可见我之前的博客：
六自由度机械臂ROS+Rviz+Arbotix控制器仿真
使用MoveIt！+Arbotix控制六自由度机械臂
MoveIt！入门：RobotModel、RobotState
MoveIt!五自由度机械臂pick_and_place抓取规划演示
使用ROS MoveIt!控制真实五自由度机械臂
下面我搞一搞这个底层部分：

标准D-H法建模

由于该机械臂只有五个自由度，并且D-H法只能实现绕Z轴的旋转和沿X轴的位移，而该臂第四个关节和第五个关节坐标系必须先绕着Z轴旋转90度，然后再绕X轴旋转90度，这是常规D-H法无法实现的。这里可以在第四个关节和第五个关节中设置一个虚拟关节，以此来过渡一下，解决上述问题。建模如下：

i	αi	ai	di	θi

正运动学Matlab

% Standard DH
% five_dof robot
% 在关节4和关节5之间增加一个虚拟关节，便于逆运动学计算
clear;
clc;
th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = pi/2;
th(2) = 0; d(2) = 0; a(2) = 0.104;alp(2) = 0;
th(3) = 0; d(3) = 0; a(3) = 0.096; alp(3) = 0;
th(4) = 0; d(4) = 0; a(4) = 0; alp(4) = 0;
th(5) = pi/2; d(5) = 0; a(5) = 0; alp(5) = pi/2;
th(6) = 0; d(6) = 0; a(6) = 0; alp(6) = 0;
th(7) = 0; d(7) = 0.163; a(7) = 0.028; alp(7) = 0;
% DH parameters  th     d    a    alpha  sigma
L1 = Link([th(1), d(1), a(1), alp(1), 0]);
L2 = Link([th(2), d(2), a(2), alp(2), 0]);
L3 = Link([th(3), d(3), a(3), alp(3), 0]);
L4 = Link([th(4), d(4), a(4), alp(4), 0]);
L5 = Link([th(5), d(5), a(5), alp(5), 0]); 
L6 = Link([th(6), d(6), a(6), alp(6), 0]);
L7 = Link([th(7), d(7), a(7), alp(7), 0]);
robot = SerialLink([L1, L2, L3, L4, L5, L6, L7]); 
robot.name='MyRobot-5-dof';
robot.display() 
theta = [0 0 0 0 90 0 0]*pi/180;
robot.teach();
robot.plot(theta); 
t = robot.fkine(theta)    %末端执行器位姿
ik_T = five_dof_ikine(t)

逆运动学推导

逆运动学Matlab

function ik_T = five_dof_ikine(fk_T)
a2 = 0.104; a3 = 0.096; ae = 0.028; de = 0.163; % ae和de即为a7、d7

nx = fk_T(1, 1); ox = fk_T(1, 2); ax = fk_T(1, 3); px = fk_T(1, 4);
ny = fk_T(2, 1); oy = fk_T(2, 2); ay = fk_T(2, 3); py = fk_T(2, 4);
nz = fk_T(3, 1); oz = fk_T(3, 2); az = fk_T(3, 3); pz = fk_T(3, 4);

% theta1
theta1 = atan2(py - ny*ae - ay*de, px - nx*ae - ax*de);
% theta5
theta5 = atan2(sin(theta1)*nx - cos(theta1)*ny, sin(theta