UR3机器人雅克比矩阵

前往我的博客阅读体验更佳：本文链接

3 雅可比矩阵

3.1 概念

  由正运动学分析可知，只要我们知道机器人的结构以及每个关节的位置（旋转关节的角度或者移动关节的平移），就可以求出末端执行器的位姿，即末端执行器的位姿和关节位置有如下关系：
$$\vec{x}=f(\vec{\theta })\text{\hspace{1em}(1)}$$
其中，$\vec{x}$表示末端执行器的位姿，$\vec{\theta }$表示关节角度。

  对式(1)两边同时对时间$t$求导可得：
$$\frac{\mathrm{d}\vec{x}}{\mathrm{d}t}=\frac{\mathrm{d}f(\vec{\theta })}{\mathrm{d}t}=\frac{\mathrm{d}f(\vec{\theta })}{\mathrm{d}\vec{\theta }}\cdot \frac{\mathrm{d}\vec{\theta }}{\mathrm{d}t}\text{\hspace{1em}(2)}$$
即：
$$\dot{x}=\frac{\mathrm{d}\vec{x}}{\mathrm{d}\vec{\theta }}\cdot \dot{\theta }=J(\theta )\cdot \dot{\theta }\text{\hspace{1em}(3)}$$

  矩阵$J(\theta )$即为机器人的雅可比矩阵，它将机器人关节空间的速度与操作空间的速度联系起来。这里需要注意$\vec{x}$，$\vec{x}$的描述有多种形式，其位置和姿态可以表示在笛卡尔坐标系、柱坐标系、球坐标系等不同坐标系下，其姿态可以表示为欧拉角、四元数等等。随着$\vec{x}$的描述不同，雅可比矩阵$J(\theta )$也会随之变化。为方便起见，定义基本雅可比矩阵，即用笛卡尔坐标描述线速度和角速度、以机械臂的基准坐标系（即坐标系 { 0 } \{0\} {0}）作为参照系来描述末端执行器速度所求得的雅可比矩阵。（如无特殊说明，以下提到的雅可比矩阵均指基本雅可比矩阵）

3.2 UR3基本雅可比矩阵的求解

  根据基本雅可比矩阵的定义，末端执行器的速度可以写为：
$$\dot{x}=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\\ {\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{c}\vec{v}\\ \vec{\omega }\end{array}\right]\text{\hspace{1em}(4)}$$
雅可比矩阵应为$6\ast 6$的矩阵，可以写成：
$$J=\left[\begin{array}{c}{J}_v\\ J_{\omega }\end{array}\right]\text{\hspace{1em}(5)}$$
上半部分对应线速度，下半部分对应角速度。

3.2.1 线速度部分

  由正运动学部分可以得到，末端执行器位姿矩阵为：
$$T_6^0=\left[\begin{array}{cccc}{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{array}\right]\text{\hspace{1em}(6)}$$

  其中$p_x$、$p_y$、$p_z$即为末端执行器在坐标系 { 0 } \{0\} {0}下的位置，因此，要求雅可比矩阵线速度部分$J_v$，只需要将$p_x$、$p_y$、$p_z$分别对关节角求导即可。所以，可求得$J_v$为：
$$J_v=\left[\begin{array}{cccccc}\frac{\mathrm{d}{p}_x}{\mathrm{d}{\theta }_1}& \frac{\mathrm{d}{p}_x}{\mathrm{d}{\theta }_2}& \frac{\mathrm{d}{p}_x}{\mathrm{d}{\theta }_3}& \frac{\mathrm{d}{p}_x}{\mathrm{d}{\theta }_4}& \frac{\mathrm{d}{p}_x}{\mathrm{d}{\theta }_5}& \frac{\mathrm{d}{p}_x}{\mathrm{d}{\theta }_6}\\ \frac{\mathrm{d}{p}_y}{\mathrm{d}{\theta }_1}& \frac{\mathrm{d}{p}_y}{\mathrm{d}{\theta }_2}& \frac{\mathrm{d}{p}_y}{\mathrm{d}{\theta }_3}& \frac{\mathrm{d}{p}_y}{\mathrm{d}{\theta }_4}& \frac{\mathrm{d}{p}_y}{\mathrm{d}{\theta }_5}& \frac{\mathrm{d}{p}_y}{\mathrm{d}{\theta }_6}\\ \frac{\mathrm{d}{p}_z}{\mathrm{d}{\theta }_1}& \frac{\mathrm{d}{p}_z}{\mathrm{d}{\theta }_2}& \frac{\mathrm{d}{p}_z}{\mathrm{d}{\theta }_3}& \frac{\mathrm{d}{p}_z}{\mathrm{d}{\theta }_4}& \frac{\mathrm{d}{p}_z}{\mathrm{d}{\theta }_5}& \frac{\mathrm{d}{p}_z}{\mathrm{d}{\theta }_6}\end{array}\right]\text{\hspace{1em}(7)}$$

3.2.2 角速度部分

  在三维空间里，角速度定义为一个指向旋转轴的向量，其方向可以由右手定则确定。因为我们定义机械臂的每个旋转关节都是绕自身的z轴旋转的，所以当一个旋转关节转速为$\omega$时，它所贡献的的末端执行器的角速度向量以这个旋转关节本身的坐标系为参照系必然为$[0,0,\omega ]$。换句话说，这个旋转关节的转速乘以$[0,0,1]$即可得到它所引起的末端执行器的角速度（在该旋转关节坐标系下）。末端执行器的实际转速可由不同旋转关节贡献的的角速度线性叠加得到。

  由于我们的基本雅可比矩阵是以坐标系 { 0 } \{0\} {0}为参照系的，为了写出$J_{\omega }$，我们需要把每个旋转关节的$z$轴$[0,0,1]$，从以关节自身坐标系为参照系转换到基准坐标系 { 0 } \{0\} {0}中表示。另外，对于平移关节，因为平移关节的运动不可能改变末端执行器的朝向，所以末端执行器的角速度对平移关节位置的求导一定是0！

  所以，雅可比矩阵角速度部分$J_{\omega }$为：
$$J_{\omega }=\left[\begin{array}{cccccc}{}^0{\hat{Z}}_1& {}^0{\hat{Z}}_2& {}^0{\hat{Z}}_3& {}^0{\hat{Z}}_4& {}^0{\hat{Z}}_5& {}^0{\hat{Z}}_6\end{array}\right]\text{\hspace{1em}(8)}$$
由正运动学部分分析可知，${}^0{\hat{Z}}_i$即为齐次坐标矩阵$T_i^0$第三列前三行元素。

3.2.3 UR3机器人雅可比矩阵解析表达式

由正运动学部分可知
$$\{\begin{array}{l}{p}_x=(s_1{c}_5-c_1{c}_{234}{s}_5)d_6+c_1{s}_{234}{d}_5+c_1{c}_{23}{a}_3+s_1{d}_4+c_1{c}_2{a}_2+s_1{d}_2\\ p_y=-(c_1{c}_5+s_1{c}_{234}{s}_5)d_6+s_1{s}_{234}{d}_5+s_1{c}_{23}{a}_3-c_1{d}_4+s_1{c}_2{a}_2-c_1{d}_2\\ p_z=-s_{234}{s}_5{d}_6-c_{234}{d}_5+s_{23}{a}_3+s_2{a}_2+d_1\end{array}\text{\hspace{1em}(9)}$$
$$T_1^0=\left[\begin{array}{cccc}\ast & \ast & 0& \ast \\ \ast & \ast & 0& \ast \\ \ast & \ast & 1& \ast \\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(10)}$$
$$T_2^0=\left[\begin{array}{cccc}\ast & \ast & s_1& \ast \\ \ast & \ast & -c_1& \ast \\ \ast & \ast & 0& \ast \\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(11)}$$
$$T_3^0=\left[\begin{array}{cccc}\ast & \ast & s_1& \ast \\ \ast & \ast & -c_1& \ast \\ \ast & \ast & 0& \ast \\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(12)}$$
$$T_4^0=\left[\begin{array}{cccc}\ast & \ast & s_1& \ast \\ \ast & \ast & -c_1& \ast \\ \ast & \ast & 0& \ast \\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(13)}$$
$$T_5^0=\left[\begin{array}{cccc}\ast & \ast & c_1{s}_{234}& \ast \\ \ast & \ast & s_1{s}_{234}& \ast \\ \ast & \ast & -c_{234}& \ast \\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(14)}$$
$$T_6^0=\left[\begin{array}{cccc}\ast & \ast & s_1{c}_5-c_1{c}_{234}{s}_5& \ast \\ \ast & \ast & -c_1{c}_5-s_1{c}_{234}{s}_5& \ast \\ \ast & \ast & -s_{234}{s}_5& \ast \\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(15)}$$
所以，UR3机器人的雅可比矩阵为：
$$J=\left[\begin{array}{cccccc}{j}_{11}& j_{12}& j_{13}& j_{14}& j_{15}& 0\\ j_{21}& j_{22}& j_{23}& j_{24}& j_{25}& 0\\ 0& j_{32}& j_{33}& j_{34}& j_{35}& 0\\ 0& s_1& s_1& s_1& c_1{s}_{234}& j_{46}\\ 0& -c_1& -c_1& -c_1& s_1{s}_{234}& j_{56}\\ 1& 0& 0& 0& -c_{234}& j_{66}\end{array}\right]\text{\hspace{1em}(16)}$$
式(16)中各简式如下：
$$\{\begin{array}{l}{j}_{11}=(c_1{c}_5+s_1{c}_{234}{s}_5)d_6-s_1{s}_{234}{d}_5-s_1{c}_{23}{a}_3+c_1{d}_4-s_1{c}_2{a}_2+c_1{d}_2\\ j_{21}=(s_1{c}_5-c_1{c}_{234}{s}_5)d_6+c_1{s}_{234}{d}_5+c_1{c}_{23}{a}_3+s_1{d}_4+c_1{c}_2{a}_2+s_1{d}_2{j}_{12}=-c_1(-s_{234}{s}_5{d}_6-c_{234}{d}_5+s_{23}{a}_3+s_2{a}_2)\\ j_{22}=-s_1(-s_{234}{s}_5{d}_6-c_{234}{d}_5+s_{23}{a}_3+s_2{a}_2)\\ j_{32}=-c_{234}{s}_5{d}_6+s_{234}{d}_5+c_{23}{a}_3+c_2{a}_2{j}_{13}=c_1(s_{234}{s}_5{d}_6+c_{234}{d}_5-s_{23}{a}_3)\\ j_{23}=s_1(s_{234}{s}_5{d}_6+c_{234}{d}_5-s_{23}{a}_3)\\ j_{33}=-c_{234}{s}_5{d}_6+s_{234}{d}_5+c_{23}{a}_3{j}_{14}=c_1(s_{234}{s}_5{d}_6+c_{234}{d}_5)\\ j_{24}=s_1(s_{234}{s}_5{d}_6+c_{234}{d}_5)\\ j_{34}=-c_{234}{s}_5{d}_6+s_{234}{d}_5{j}_{15}=-(s_1{s}_5+c_1{c}_{234}{c}_5)d_6\\ j_{25}=(c_1{s}_5-s_1{c}_{234}{c}_5)d_6\\ j_{35}=-s_{234}{c}_5{d}_6{j}_{46}=s_1{c}_5-c_1{c}_{234}{s}_5\\ j_{56}=-c_1{c}_5-s_1{c}_{234}{s}_5\\ j_{66}=-s_{234}{s}_5\end{array}\text{\hspace{1em}(17)}$$

3.3 验证

  利用Robotics Toolbox进行验证，当关节角为$\Theta ={\left[15^{\circ }15^{\circ }15^{\circ }15^{\circ }15^{\circ }15^{\circ }\right]}^T$时，分别利用Robotics Toolbox中R.jacob0()函数以及自己编写函数进行求解，所得结果如下图所示：

  从图中可以看出，利用Robotics Toolbox中R.jacob0()函数和自编函数计算结果相同，说明3.2.3节中雅可比矩阵解析表达式求解无误。