# 毕设(2)——机械臂正逆运动学分析

## 正向运动学分析

\begin{aligned} {^{i-1}_iT} &= T_{\hat{X}_{i-1}}(\alpha_{i-1}) T_{\hat{X}_R}(\alpha_{i-1}) T_{\hat{Z}_Q}(\theta_i) T_{\hat{Z}_P}(d_i) \\ &= \begin{bmatrix} c\theta_i & -s\theta_i & 0 & a_{i-1} \\ s\theta_ic\alpha_{i-1} & c\theta_ic\alpha_{i-1} & -s\alpha_{i-1} & -s\alpha_{i-1}d_i \\ s\theta_is\alpha_{i-1} & c\theta_is\alpha_{i-1} & c\alpha_{i-1} & c\alpha_{i-1}d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}

${^0_1T}= \begin{bmatrix} c\theta_1 & -s\theta_1 & 0 & 0 \\ s\theta_1 & c\theta_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} {^1_2T}= \begin{bmatrix} c\theta_2 & -s\theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ -s\theta_2 & -c\theta_2 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} {^2_3T}= \begin{bmatrix} c\theta_3 & -s\theta_3 & 0 & a_2 \\ s\theta_3 & c\theta_3 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

${^3_4T}= \begin{bmatrix} c\theta_4 & -s\theta_4 & 0 & a_3 \\ s\theta_4 & c\theta_4 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} {^4_5T}= \begin{bmatrix} c\theta_5 & -s\theta_5 & 0 & a_4 \\ 0 & 0 & 0 & d_5 \\ -s\theta_5 & -c\theta_5 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

${^0_5T}={^0_1T}{^1_2T}{^2_3T}{^3_4T}{^4_5T}= \begin{bmatrix} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix}$

${^1_4T}={^1_2T}{^2_3T}{^3_4T}= \begin{bmatrix} c_{234} & -s_{234} & 0 & c_{23}a_3+c_2a_2 \\ 0 & 0 & 1 & 0 \\ -s_{234} & -c_{234} & 0 & -s_{23}a_3-s_2a_2 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

${^1_4T}={^1_2T}{^2_3T}{^3_4T}= \begin{bmatrix} 1 & 0 & 0 & c_{23}a_3+c_2a_2 \\ 0 & 0 & 1 & 0 \\ 0 & -1 & 0 & -s_{23}a_3-s_2a_2 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

\begin{aligned} {^0_5T}={^0_1T}{^1_2T}{^2_3T}{^3_4T}{^4_5T}={^0_1T}{^1_4T}{^4_5T} &= \begin{bmatrix} c_1c_5+s_1s_5 & -c_1s_5+s_1c_5 & 0 & c_1(c_{23}a_3+c_2a_2+a_4) \\ s_1c_5-c_1s_5 & -s_1s_5-c_1c_5 & 0 & s_1(c_{23}a_3+c_2a_2+a_4) \\ 0 & 0 & -1 & -d_5-s_{23}a_3-s_2a_2 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}

## 逆向运动学分析

### 求$$\theta_1$$

$$\theta_1$$可以通过$$p_x$$$$p_y$$求出

$$\left\{ \begin{array}{l} c_1(c_{23}a_3+c_2a_2+a_4)=p_x \\ s_1(c_{23}a_3+c_2a_2+a_4)=p_y \end{array} \right.$$

### 求$$\theta_3$$

$$\left\{ \begin{array}{l} c_1(c_{23}a_3+c_2a_2+a_4)=p_x \\ s_1(c_{23}a_3+c_2a_2+a_4)=p_y \end{array} \right.$$

$$-d_5-s_{23}a_3-s_2a_2=p_z$$
$$(s_{23}a_3+s_2a_2)^2=(p_z+d_5)^2$$

$(c_{23}a_3+c_2a_2)^2+(s_{23}a_3+s_2a_2)^2=(p_x-c_1a_4)^2+(p_y-s_1a_4)^2+(p_z+d_5)^2$

$c_3=\frac{p_x^2+p_y^2+p_z^2-2a_4(c_1p_x+s_1p_y)+2p_zd_5+a_4^2+d_5^2-a_2^2-a_3^2}{2a_2a_3}$

$$\theta_3=\pm arccos(\frac{p_x^2+p_y^2+p_z^2-2a_4(c_1p_x+s_1p_y)+2p_zd_5+a_4^2+d_5^2-a_2^2-a_3^2}{2a_2a_3})$$

### 求$$\theta_2$$

$$-d_5-s_{23}a_3-s_2a_2=p_z$$
$$s_{23}$$通过和差化积公式拆开
$$(c_3a_3+a_2)s_2+(s_3a_3)c_2=-(p_z+d_5)$$

$\sqrt{(c_3a_3+a_2)^2+(s_3a_3)^2}sin(\theta_2+arctan(\frac{s_3a_3}{c_3a_3+a_2}))=-(p_z+d_5)$

$$\theta_2=arcsin(\frac{-(p_z+d_5)}{\sqrt{(c_3a_3+a_2)^2+(s_3a_3)^2}})-arctan(\frac{s_3a_3}{c_3a_3+a_2})$$

### 求$$\theta_4$$

$$\theta_4$$可根据$$\theta_2+\theta_3+\theta_4\equiv0$$求出

$$\theta_4=-(\theta_2+\theta_3)$$

### 求$$\theta_5$$

$$\left\{ \begin{array}{l} c_1c_5+s_1s_5=n_x \\ s_1c_5-c_1s_5=n_y \end{array} \right.$$
$$\theta_5=\theta_1-arctan(\frac{n_y}{n_x})$$

### 逆向运动学方程

$$\left\{ \begin{array}{l} \theta_1=arctan(\frac{p_y}{p_x}) \\ \theta_2=arcsin(\frac{-(p_z+d_5)}{\sqrt{(c_3a_3+a_2)^2+(s_3a_3)^2}})-arctan(\frac{s_3a_3}{c_3a_3+a_2}) \\ \theta_3=\pm arccos(\frac{p_x^2+p_y^2+p_z^2-2a_4(c_1p_x+s_1p_y)+2p_zd_5+a_4^2+d_5^2-a_2^2-a_3^2}{2a_2a_3}) \\ \theta_4=-(\theta_2+\theta_3) \\ \theta_5=\theta_1-arctan(\frac{n_y}{n_x}) \end{array} \right.$$

## Matlab代码验证

### 正向运动学

myfkine.m

function [T05] = myfkine(Angle_T)
%运动学正解
theta1 = Angle_T(1, 1);
theta2 = Angle_T(1, 2);
theta3 = Angle_T(1, 3);
theta4 = Angle_T(1, 4);
theta5 = Angle_T(1, 5);
theta23 = theta2 + theta3;
a2 = 135; a3 = 147; a4 = 61; d5 = 131;
T01 = [cos(theta1) -sin(theta1) 0 0;
sin(theta1) cos(theta1)  0 0;
0           0            1 0;
0           0            0 1];
T14 = [1 0  0 cos(theta23) * a3 + cos(theta2) * a2;
0 0  1 0;
0 -1 0 -sin(theta23) * a3 - sin(theta2) * a2;
0 0  0 1];
T45 = [cos(theta5)  -sin(theta5) 0 a4;
0            0            1 d5;
-sin(theta5) -cos(theta5) 0 0;
0            0            0 1];
T05 = T01 * T14 * T45;


### 验证程序

test2.m

% Modified DH 建模dobot
clear, clc, close all;

%建立机器人模型
%           关节角   关节偏距  连杆长度   连杆转角   旋转关节   偏差
%           theta    d        a         alpha     sigma      offset
L(1) = Link([0       0        0         0         0          0], 'modified');
L(2) = Link([0       0        0         -pi / 2   0          0], 'modified');
L(3) = Link([0       0        135       0         0          0], 'modified');
L(4) = Link([0       0        147       0         0          0], 'modified');
L(5) = Link([0       131      61        -pi / 2   0          0], 'modified');

%连接连杆

angle1 = [-pi / 6, -pi / 6, pi / 3, -pi / 6, pi / 4];
angle2 = [pi / 3, -pi / 60, pi / 30, -pi / 60, -pi / 4];

%运动学正解
fk1 = myfkine(angle1); %正解函数
fk2 = robot.fkine(angle1);%工具箱正解函数
%fk1 = myfkine(angle2); %正解函数
%fk2 = robot.fkine(angle2);%工具箱正解函数

%运动学逆解
ik1 = myikine(fk1);%逆解函数
%实际上robot.ikine函数并不可用
%ik2 = robot.ikine(fk1);%工具箱逆解函数