【机器人2】基于POE公式的UR5机械臂逆运动学建模求解与matlab仿真

---

上一篇：【机器人1】基于POE公式的UR5机械臂正运动学建模求解与matlab仿真

---

基于PoE公式的UR5机械臂逆运动学分析

1.1 理论基础

1.1.1 一阶运动学

讨论如何从给定的一组关节位置和速度计算末端执行器的速度（运动旋量）。

• $n$杆开链机器人的正向运动学可以写成PoE公式$$\mathbf{T}(\mathbf{\theta })=e^{\left[{\mathcal{S}}_1\right]{\theta }_1}{e}^{\left[{\mathcal{S}}_2\right]{\theta }_2}\dots e^{\left[{\mathcal{S}}_n\right]{\theta }_n}\mathbf{M}$$空间雅可比 $J_s(\mathbf{\theta })\in {\mathbb{R}}^{6\times n}$ 通过公式 ${\mathcal{V}}_s=J_s(\mathbf{\theta })\dot{\mathbf{\theta }}$ 联系起关节速度 $\dot{\mathbf{\theta }}\in {\mathbb{R}}^n$ 与空间速度 ${\mathcal{V}}_s\in {\mathbb{R}}^6$，$J_s(\mathbf{\theta })$第$i$列为$$\begin{array}{l}{J}_{si}(\mathbf{\theta })={\mathrm{Ad}}_{e^{\left[S_1\right]{\theta }_1\dots }{e}^{\left[S_{i-1}\right]{\theta }_{i-1}}}\left({\mathcal{S}}_i\right)i=2,\cdots ,n\\ J_{s1}={\mathcal{S}}_1\end{array}$$$J_{si}$为机器人经历刚体位移${\mathbf{T}}_{i-1}=e^{\left[{\mathcal{S}}_1\right]{\theta }_1}\cdots e^{\left[{\mathcal{S}}_{i-1}\right]{\theta }_{i-1}}$之后第$i$个关节轴相对空间坐标系的旋量，同时也是关节变量${\theta }_1\cdots {\theta }_{i-1}$的函数。
• $n$杆开链机器人的正向运动学也可以写成如下PoE公式$$\mathbf{T}(\mathbf{\theta })=\mathbf{M}{e}^{\left[{\mathcal{B}}_1\right]{\theta }_1}{e}^{\left[{\mathcal{B}}_2\right]{\theta }_2}\cdots e^{\left[{\mathcal{B}}_n\right]{\theta }_n}$$物体雅可比 $J_b(\mathbf{\theta })\in {\mathbb{R}}^{6\times n}$ 通过公式 ${\mathcal{V}}_b=J_b(\mathbf{\theta })\dot{\mathbf{\theta }}$ 联系起关节速度 $\dot{\mathbf{\theta }}\in {\mathbb{R}}^n$ 与末端物体速度 ${\mathcal{V}}_b\in {\mathbb{R}}^6$，$J_b(\mathbf{\theta })$第$i$列为$$\begin{array}{l}{J}_{bi}(\mathbf{\theta })={\mathrm{Ad}}_{e^{-[B_n]{\theta }_n\dots }{e}^{-\left[B_{i+1}\right]{\theta }_{i+1}}}\left({\mathcal{B}}_i\right)i=n-1,\cdots ,1\\ J_{bn}={\mathcal{B}}_n\end{array}$$$J_{bi}$为第$i$个关节轴的旋量坐标，相对末端坐标系来描述。$\mathbf{\theta }$值任意。

1.1.2 牛顿-拉夫森数值迭代算法

此方法可以用于求解非线性方程。

1.2 基于PoE公式的UR5机械臂逆运动学求解

UR5机械臂尺寸数据及螺旋轴的建立如下图左、右所示。

空间坐标系下的螺旋轴${\mathcal{S}}_i=\left({\mathbf{\omega }}_i,{\mathbf{v}}_i\right),i=1,\dots ,6$，坐标如下表所示：

$i$	${\mathbf{\omega }}_i$	${\mathbf{v}}_i$
1	$(0,0,1)$	$(0,0,0)$
2	$(0,-1,0)$	$\left(H_1,0,0\right)$
3	$(0,-1,0)$	$\left(H_1,0,L_1\right)$
4	$(0,-1,0)$	$\left(H_1,0,L_1+L_1\right)$
5	$(0,0,-1)$	$\left(W_1,-L_1-L_1,0\right)$
6	$(0,-1,0)$	$\left(H_1-H_1,0,L_1+L_1\right)$

逆运动学方程无解析解时，可采用迭代数值方法求解。即使存在解析解，数值算法也可用来改善求解精度。本节将运动学逆解方程转化为数值方法可解的形式。
由于建立了空间坐标系下描述的螺旋轴，因此方便起见，采用空间速度旋量参与求解，算法如下：

1.3 matlab仿真验证

首先进行数据准备，包括空间坐标系下的螺旋轴、末端初始位形、期望末端位形（对应关节角${\mathbf{\theta }}_1=\left(0,-\frac{2\pi }{3},-\frac{\pi }{2},-\frac{\pi }{4},-\frac{\pi }{4},0\right)$）、给数值迭代的初始关节角、容忍误差：

IKinSpace：逆运动学数值迭代求解，返回求解θ结果和成功标志s。

FKinSpace：空间坐标系下的正运动学求解，见上一篇文章。
Adjoint：求伴随变换矩阵。

MatrixLog6：将变换矩阵T转化为T的矩阵对数形式。

MatrixLog3：将旋转矩阵R转化为R的矩阵对数形式。

JacobianSpace：给定空间坐标系下的关节旋量和关节角，计算空间雅可比。

求解结果如下：
期望位形的关节角：

牛顿-拉夫森法求逆解得到的关节角：

可以看出，求解结果接近期望关节角，求解成功。
可视化此关节角下的UR5机械臂位形：

左边是期望关节角对应的期望位形，右边是求解关节角对应的位形。

---

上一篇：【机器人1】基于POE公式的UR5机械臂正运动学建模求解与matlab仿真

---