智能机器人技术作业记录

智能机器人技术——机器人运动学奇异分析与性能评价

一、给定平面2R机械臂状态参数，

状态描述：

• 关节状态: \(\quad\left[\theta_{1}, \theta_{2}\right]^{\mathrm{T}}\)
• 末端位置: \(\quad\left[x_{e}, y_{e}\right]^{\mathrm{T}}\)

1. 计算逆运动学，求解关节角的表达式（已知末端位置\(\quad\left[x_{e}, y_{e}\right]^{\mathrm{T}}\)，求关节角\(\quad\left[\theta_{1}, \theta_{2}\right]^{\mathrm{T}}\)）

   根据几何关系, 可推导出机械臂末端位置与机械臂关节变量的关系:

   \[ \begin{array}{l} p_{\mathrm{ex}}=l_{1} c_{1}+l_{2} c_{12} \\ p_{\mathrm{ey}}=l_{1} s_{1}+l_{2} s_{12} \end{array} \]

   其中,

   \[ \left\{\begin{array}{l} s_{1}=\sin \theta_{1}, c_{1}=\cos \theta_{1} \\ s_{12}=\sin \left(\theta_{1}+\theta_{2}\right), c_{12}=\cos \left(\theta_{1}+\theta_{2}\right) \end{array}\right. \]

   其向量形式为：

   \[ p_{\mathrm{e}}=\left[\begin{array}{l} p_{\mathrm{ex}} \\ p_{\mathrm{e} y} \end{array}\right]=\left[\begin{array}{l} l_{1} c_{1}+l_{2} c_{12} \\ l_{1} s_{1}+l_{2} s_{12} \end{array}\right]=\left[\begin{array}{l} l_{1} \cos \theta_{1}+l_{2} \cos \left(\theta_{1}+\theta_{2}\right) \\ l_{1} \sin \theta_{1}+l_{2} \sin \left(\theta_{1}+\theta_{2}\right) \end{array}\right] \]

   将式子两边的平方相加, 有

   \[ p_{\mathrm{e}}^{2}=l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2}\left(c_{1} c_{12}+s_{1} s_{12}\right) \]

   \(p_{\mathrm{e}}^{2}=p_{\mathrm{ex}}^{2}+p_{\mathrm{ey}}^{2}\) 为基坐标系原点到末端坐标系原点的距离。

   根据三角函数的性质, 有

   \[ c_{1} c_{12}+s_{1} s_{12}=c_{2} \]

   故可化简为

   \[ p_{\mathrm{e}}^{2}=l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2} c_{2} \]

   因此，

   \[ \left\{\begin{array}{l} p_{\mathrm{e}}^{2} \leqslant l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2}=\left(l_{1}+l_{2}\right)^{2} \\ p_{\mathrm{e}}^{2} \geqslant l_{1}^{2}+l_{2}^{2}-2 l_{1} l_{2}=\left(l_{1}-l_{2}\right)^{2} \end{array}\right.\\ \left|l_{1}-l_{2}\right| \leqslant p_{e} \leqslant l_{1}+l_{2} \]

   上式即表示了该 \(2 \mathrm{R}\) 机械臂的工作空间范围, 其最小边沿和最大边沿分别对应于 \(\theta_{2}=\pi\) 和 \(\theta_{2}=0\) 的情况。

   进一步有，

   \[ \left|\frac{p_{\mathrm{e}}^{2}-l_{1}^{2}-l_{2}^{2}}{2 l_{1} l_{2}}\right| \leqslant 1 \]

   解得：

   \[ \theta_{2}=\pm \arccos \frac{p_{\mathrm{e}}^{2}-l_{1}^{2}-l_{2}^{2}}{2 l_{1} l_{2}}=\pm \arccos \frac{x_{\mathrm{e}}^{2}+y_{\mathrm{e}}^{2}-l_{1}^{2}-l_{2}^{2}}{2 l_{1} l_{2}} \]

   关节角 \(\theta_{2}\) 解出后, 将其代入方程组中, 可进一步解出关节角 \(\theta_{1}\) 。首先根据三角函数的性质:

   \[ \begin{aligned} &c_{12}=c_{1} c_{2}-s_{1} s_{2} \\ &s_{12}=s_{1} c_{2}+c_{1} s_{2} \end{aligned} \]

   有，

   \[ \left\{\begin{array}{l} \left(l_{1}+l_{2} c_{2}\right) c_{1}-l_{2} s_{2} s_{1}=p_{\mathrm{ex}} \\ l_{2} s_{2} c_{1}+\left(l_{1}+l_{2} c_{2}\right) s_{1}=p_{\mathrm{e} y} \end{array}\right. \]

   可写成如下形式:

   \[ \left[\begin{array}{cc} l_{1}+l_{2} c_{2} & -l_{2} s_{2} \\ l_{2} s_{2} & l_{1}+l_{2} c_{2} \end{array}\right]\left[\begin{array}{l} c_{1} \\ s_{1} \end{array}\right]=\left[\begin{array}{l} p_{e x} \\ p_{\mathrm{ey}} \end{array}\right] \]

   方程组的系数矩阵 \(\boldsymbol{A}\) 及其行列式分别为

   \[ \boldsymbol{A}=\left[\begin{array}{cc} l_{1}+l_{2} c_{2} & -l_{2} s_{2} \\ l_{2} s_{2} & l_{1}+l_{2} c_{2} \end{array}\right], \operatorname{det}(\boldsymbol{A})=l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2} c_{2} \]

   若 \(\operatorname{det}(\boldsymbol{A}) \neq 0\), 则矩阵 \(\boldsymbol{A}\) 满秩, 方程组有解, 即:

   \[ \left[\begin{array}{l} c_{1} \\ s_{1} \end{array}\right]=\left[\begin{array}{cc} l_{1}+l_{2} c_{2} & -l_{2} s_{2} \\ l_{2} s_{2} & l_{1}+l_{2} c_{2} \end{array}\right]^{-1}\left[\begin{array}{l} p_{\mathrm{ex}} \\ p_{\mathrm{ey}} \end{array}\right]=\frac{1}{l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2} c_{2}}\left[\begin{array}{c} \left(l_{1}+l_{2} c_{2}\right) p_{\mathrm{ex}}+l_{2} s_{2} p_{\mathrm{ey}} \\ -l_{2} s_{2} p_{\mathrm{ex}}+\left(l_{1}+l_{2} c_{2}\right) p_{\mathrm{ey}} \end{array}\right] \]

   \(\theta_{1}\) 可根据解出的 \(s_{1}\) 和 \(c_{1}\) 求出, 即:

   \[ \begin{aligned} \theta_{1} &=\arctan 2\left(s_{1}, c_{1}\right)=\arctan 2\left(\frac{-l_{2} s_{2} p_{\mathrm{ex}}+\left(l_{1}+l_{2} c_{2}\right) p_{\mathrm{ey}}}{l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2} c_{2}}, \frac{\left(l_{1}+l_{2} c_{2}\right) p_{\mathrm{ex}}+l_{2} s_{2} p_{\mathrm{ey}}}{l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2} c_{2}}\right) \\ &=\arctan 2\left(-l_{2} s_{2} p_{\mathrm{ex}}+\left(l_{1}+l_{2} c_{2}\right) p_{\mathrm{ey}},\left(l_{1}+l_{2} c_{2}\right) p_{\mathrm{ex}}+l_{2} s_{2} p_{\mathrm{ey}}\right) \end{aligned} \]

   根据基本不等式及三级函数的性质, 令 \(\operatorname{det}(\boldsymbol{A})=0\), 则必有 \(l_{1}=l_{2}\) 且 \(\theta_{2}=\pi\), 即:

   \[ \operatorname{det}(\boldsymbol{A})=0 \Rightarrow\left\{\begin{array} { l } { l _ { 1 } ^ { 2 } + l _ { 2 } ^ { 2 } + 2 l _ { 1 } l _ { 2 } c _ { 2 } = 0 } \\ { l _ { 1 } ^ { 2 } + l _ { 2 } ^ { 2 } \geqslant 2 l _ { 1 } l _ { 2 } } \\ { | c _ { 2 } | \leqslant 1 } \end{array} \Rightarrow \left\{\begin{array}{l} l_{1}=l_{2} \\ \theta_{2}=\pi \end{array}\right.\right. \]

   综上所述, 结果可以求出两组解, 对应于机器人的两种臂型, 分别称为高臂 (肘) 和低臂 (肘), 平面 \(2 \mathrm{R}\) 机械臂逆运动学多解情况分析下图所示。也就是说, 对于前述的 \(2 \mathrm{R}\) 机械臂, 当给定末端点的一个位置 \(\boldsymbol{p}_{\mathrm{e}}\) 时, 有两组关节角与之对应, 即位置级逆运动学有多解。

   

2. 计算雅克比矩阵

根据机械臂末端位置与机械臂关节变量的关系求导得：

\[ \left\{\begin{array}{l} \dot{p}_{\mathrm{ex}}=-l_{1} s_{1} \dot{\theta}_{1}-l_{2} s_{12}\left(\dot{\theta}_{1}+\dot{\theta}_{2}\right)=-\left(l_{1} s_{1}+l_{2} s_{12}\right) \dot{\theta}_{1}-l_{2} s_{12} \dot{\theta}_{2} \\ \dot{p}_{\mathrm{cy}}=l_{1} c_{1} \dot{\theta}_{1}+l_{2} c_{12}\left(\dot{\theta}_{1}+\dot{\theta}_{2}\right)=\left(l_{1} c_{1}+l_{2} c_{12}\right) \dot{\theta}_{1}+l_{2} c_{12} \dot{\theta}_{2} \\ \end{array}\right. \]

即:

\[ \left[\begin{array}{l} \dot{p}_{\mathrm{ex}} \\ \dot{p}_{\mathrm{ey}} \end{array}\right]=\left[\begin{array}{cc} -l_{1} s_{1}-l_{2} s_{12} & -l_{2} s_{12} \\ l_{1} c_{1}+l_{2} c_{12} & l_{2} c_{12} \end{array}\right]\left[\begin{array}{l} \dot{\theta}_{1} \\ \dot{\theta}_{2} \end{array}\right] \]

其矩阵形式为：

\[ \dot{\boldsymbol{p}}_{\mathrm{e}}=\boldsymbol{J}_{v}(\boldsymbol{\Theta}) \dot{\boldsymbol{\Theta}} \]

此时, \(\boldsymbol{J}_{v}\) 为 \(2 \times 2\) 的方阵:

\[ \boldsymbol{J}_{v}=\left[\begin{array}{cc} -l_{1} s_{1}-l_{2} s_{12} & -l_{2} s_{12} \\ l_{1} c_{1}+l_{2} c_{12} & l_{2} c_{12} \end{array}\right] \]

二、给定D-H坐标系，填写D-H参数表。

\[ \begin{array}{ccccc} \hline \text { 连杆i } & \theta_{i} & \alpha_{i} & a_{i} & d_{i} \\ \hline1 & 0 & -90^{\circ} & 0 & d_{1} \\ 2 & 0 & 0 & a_{2} & 0 \\ 3 & 0 & 0 & a_{3} & 0\\ \hline \end{array} \]