【机器人】基于指数积的机械臂运动学标定

基于局部POE的机器人运动学标定

导语：定位精度作为影响机器人性能的重要因素，需要对机器人进行标定来提高机器人的定位精度从而满足工业上的需要。
Question： 什么是机器人标定？为什么机器人需要标定？
Answer：机器人标定是指机器人运动学标定，为了使机器人本体达到与理论设计模型一致，提高机器人的点精度和轨迹精度或者能够完全以编程设定的动作运动，机器人在装配后需要进行标定工作。标定的方法主要有激光跟踪仪、拉线法、20点法等方法。

一. 标定过程

基于误差模型的标定方法可以提高机器人在整个空间里的精度。误差模型法通常分为四步：
1.建模：根据机器人的结构建立机器人运动学模型。
2.测量：利用外部设备测量末端执行器在某一坐标系下的实际位姿。
3.参数辨识：将机器人末端执行器的位姿误差代入误差模型得到机器人的几何参数误差。
4.参数补偿：将机器人的参数修改成计算出来的实际运动学参数。

1.1 建模

  建模方法使用指数积建立机器人的运动学模型。基于指数积的机械臂正运动学 。这种建模方法是全局POE运动学建模，之所以使用指数积建模是因为使用DH法建模时不满足完整性和连续性，而POE建模可以满足其标定模型的要求。

1.2 测量

  视觉捕捉Nokov红外测量或者使用激光传感器测量末端执行器在测量原坐标系下的位姿信息。

1.3 参数辨识

   机器人运动学参数辨识，采用的方法使用递归最小二乘法。

1.4 参数补偿

  几何参数补偿：这里采用局部POE方法标定，局部POE标定并不是还原实际的机器人几何参数，而是为了得到一台绝对精度更加准确的机器人，我们可以在标定完成之后更改机器人输入关节角使机器人走到理想的位置。

二. 基于局部POE的机器人正向运动学建模

旋量理论数学基础
全局POE运动学公式：(考虑末端执行器)
$$T\left(\theta \right)=e^{\left[S_1\right]{\theta }_1}\cdots e^{\left[S_{n-1}\right]{\theta }_{n-1}}{e}^{\left[S_n\right]{\theta }_n}M{T}_{n,n+1}$$其中 $T_{n,n+1}$ 为末端执行器到第六关节法兰盘中心的变换矩阵。
局部POE运动学公式：将初始矩阵$M$改写成 $$M=T_{0,1}\left(0\right)T_{1,2}\left(0\right)T_{2,3}\left(0\right)T_{3,4}\left(0\right)T_{4,5}\left(0\right)T_{5,6}\left(0\right)$$ $T_{0,1}\left(0\right)$ 代表第一关节到第二关节的初始位姿变换矩阵。以此类推，有局部POE公式
$$T\left(\theta \right)=T_{0,1}\left(0\right)e^{\left[S_1\right]{\theta }_1}\cdots \cdots T_{n-1,n}\left(0\right)e^{\left[S_n\right]{\theta }_n}{T}_{n,n+1}\left(0\right)$$   由此可知，局部POE需要再每个关节上建立坐标系，并在此坐标系下设置关节旋量。坐标系的建立只需得到关节坐标系变换关系即可。

三. 基于局部POE的误差建立方法

3.1 采用位姿测量的误差模型

定理：对于一个给定的转换矩阵 $T\in SE\left(3\right)$ ，至少存在一个旋量 $\stackrel{\wedge }{p}\in se\left(3\right)$ 使得 $e^{{\stackrel{\wedge }{p}}_i}=T$，因此可以令 $e^{{\stackrel{\wedge }{p}}_i}=T_{i-1,i}\left(0\right)$ ，那么局部POE公式可以改写为$$T\left(\theta \right)=T_{0,n+1}=e^{\stackrel{\wedge }{p_1}}{e}^{\left[S_1\right]{\theta }_1}\cdots \cdots e^{\stackrel{\wedge }{p_n}}{e}^{\left[S_n\right]{\theta }_n}{e}^{\stackrel{\wedge }{p_{n+1}}}$$

   对上式求导并经过处理，将 旋量$S$ 和 角度$\theta$ 看作是初始矩阵 $T\left(0\right)$ 的一个误差源。有局部POE的误差模型：
$$y=Jx$$ 其中
$$\begin{gathered} y=\log {\left(T_{0,n+1}^a,T_{0,n+1}^{-1}\right)}^{\vee }\in R^{6\times 1} \\ x={\left[\delta p_1,\delta p_2,\delta p_3,\delta p_4,\delta p_5,\delta p_6\right]}^T\in R^{6\left(n+1\right)\times 6} \\ J=\left[Ad\left(T_{0,1}\left(0\right),\right),Ad\left(T_{0,1}\right)Ad\left(T_{1,2}\left(0\right),\right),...Ad\left(T_{0,n}\left(0\right),\right)Ad\left(T_{n,n+1}\left(0\right),\right)\right]\in R^{6\left(n+1\right)\times 6} \end{gathered}$$

$T_{0,n+1}^{-1}$ 和 $J$ 可以用机器人名义运动学模型得到，而 $T_{0,n+1}^a$ 可以通过实际测量数据得到，因此位姿误差 $y$ 可以通过计算可得，$x$ 就是需要辨识的运动学参数误差。
辨识方程：$$x=J^{-1}y$$

基于局部POE法参数更新方法为$$T_{i-1,i}^{new}\left(0\right)=T_{i-1,i}^{old}\left(0\right)e^{\delta {\stackrel{\wedge }{p}}_i}$$按照以上方法对参数误差进行辨识处理，当误差满足要求时迭代结束。

注：此种方法是让理论坐标靠近实际坐标，此初始矩阵包含了位姿误差信息。标定点数至少测量14组点数，20点最好。
   对于一个点 $y=Jx$ 矩阵维数是 6 * 1 = 6 * 42 * 42 * 1；
   多个点时 $y=Jx$ 矩阵维数是 6 * n = 6 * 42 * 42 * n;
   通过基于局部 POE 辨识出来的参数，虽然也不是真实参数，但是却是一组与真实参数效果相同的参数，即对于同一组关节角度，无论是采用真实参数还是采用基于局部 POE 辨识出来的参数，都将到达同一位姿。

四. 参数补偿

4.1 基于局部 POE 的关节角数值解法

  在得到标定后的初始矩阵 $T_{i-1,i}\left(0\right)$ 后，将机器人的控制器中的关节角作为初始值代入迭代算出每个关节角应该补偿的值，再输入到机器人控制器中改变实际关节角值，使实际坐标值靠近理论坐标值。
$$y=Jx$$ 其中
$$\begin{gathered} y=\log {\left(T_{0,n+1}^a,T_{0,n+1}^{-1}\right)}^{\vee }\in R^{6\times 1} \\ x={\left[\delta q_1,\delta q_2,\delta q_3,\delta q_4,\delta q_5,\delta q_6\right]}^T\in R^{6\left(n+1\right)\times 6} \\ J=\left[Ad\left(T_{0,1}\left(0\right)\right){\xi }_1,Ad\left(T_{0,1}\right)Ad\left(T_{1,2}\left(0\right)\right){\xi }_2,...Ad\left(T_{0,n-1}\left(0\right)\right)Ad\left(T_{n-1,n}\left(0\right)\right){\xi }_n\right]\in R^{6n\times 6} \end{gathered}$$
   辨识方程： $x=J^{-1}y$ 。将计算的关节误差值加到初始关节值上再输入给控制器，此时机械臂实际位置 $T_{0,n+1}^a$ 改变，方程的更新值为 $y$，不断迭代，满足误差时停止，即达到实际坐坐标值与理论值吻合，实现标定。此种方法需要控制器配合使用。

算例：6R型开链机器人

设理论位置为关节角为[0.1;0.1;0.1;0.1;0.1;0.1]的位姿

参数设置：设置位姿误差 $\delta y$ 小于1e-8时停止迭代，循环100次，迭代不出来视为求解失败。
运算结果：

在迭代26次后，停止了迭代，此时位姿成功迭代到实际位姿，再继续求逆解得到实际的关节角为[0.11;0.11;0.11;0.11;0.11;0.11]，再将这组关节角输入给控制器，控制器根据这组关节角算出来的名义位置更加逼近于真实位姿，即使理论靠近实际，此时标定完成，显著地提高了机器人的绝对精度。

注：标定成功的最直接的验证方法是在机器人基坐标系下得到的测量位置与名义位置是否相同。在标定完成后，可以给机器人输入关节角观察机器人的名义位置和实际位置是否足够接近来观察标定是否成功。