机器人运动学参数辨识（DH参数误差标定）

0. 前言

用于辨识几何参数误差的方法众多，其中较为常用的是最小二乘法和智能搜索算法。本文基于MDH参数建立两岸坐标系，不清楚的可以参考机器人修正DH参数(MDH)和标准DH(SDH)参数这篇文章

1. 全微分几何参数误差模型

根据建立的机器人MDH模型，机器人相邻关节坐标系之间的齐次变换矩阵表示为：
$$\begin{array}{rl}{}_i^{i-1}T& =R_X({\alpha }_{i-1})D_X(a_{i-1})R_Z({\theta }_i)D_Z(d_i)\\ & =\left[\begin{array}{cccc}c{\theta }_i& -s{\theta }_i& 0& a_{i-1}\\ s{\theta }_{i}c{\alpha }_{i-1}& c{\theta }_{i}c{\alpha }_{i-1}& -s{\alpha }_{i-1}& -s{\alpha }_{i-1}{d}_i\\ s{\theta }_{i}s{\alpha }_{i-1}& c{\theta }_{i}s{\alpha }_{i-1}& c{\alpha }_{i-1}& c{\alpha }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]\end{array}\text{\hspace{1em}(1)}$$
如果按照上述方法将连杆坐标系固定于连杆上时，连杆的几何参数可以定义为

1. $a_{i-1}$：沿${\hat{X}}_{i-1}$轴，从${\hat{Z}}_{i-1}$移动到${\hat{Z}}_i$的距离，通常取正值；
2. ${\alpha }_{i-1}$：绕${\hat{X}}_{i-1}$轴，从${\hat{Z}}_{i-1}$旋转到${\hat{Z}}_i$的角度，采用右手法则判断正负；
3. $d_i$：沿${\hat{Z}}_i$轴，从${\hat{X}}_{i-1}$移动到${\hat{X}}_i$的距离；
4. ${\theta }_i$：绕${\hat{Z}}_i$轴，从${\hat{X}}_{i-1}$旋转到${\hat{X}}_i$的角度，采用右手法则判断正负；

几何参数的微小误差影响各关节的齐次变换矩阵，设四个运动学参数的微小误差值为$\delta {\theta }_i$,$\delta d_i$,$\delta {\alpha }_{i-1}$,$\delta a_{i-1}$。
对旋转变换矩阵进行全微分，由于几何参数误差本身就属于微小误差，忽略高阶误差得：
$$d{(}_i^{i-1}T)=\frac{{\partial }_i^{i-1}T}{\partial a_{i-1}}\Delta a_{i-1}+\frac{{\partial }_i^{i-1}T}{\partial {\alpha }_{i-1}}\Delta {\alpha }_{i-1}+\frac{{\partial }_i^{i-1}T}{\partial d_i}\Delta d_i+\frac{{\partial }_i^{i-1}T}{\partial {\theta }_i}\Delta {\theta }_i$$
机器人第i关节位置误差和几何参数的关系：
$$d(P_i)=\frac{\partial P_i}{\partial a_{i-1}}\Delta a_{i-1}+\frac{\partial P_i}{\partial {\alpha }_{i-1}}\Delta {\alpha }_{i-1}+\frac{\partial P_i}{\partial d_i}\Delta d_i+\frac{\partial P_i}{\partial {\theta }_i}\Delta {\theta }_i$$
机器人第i关节姿态误差和几何参数的关系：
$${\delta }_i=\frac{\partial O_i}{\partial a_{i-1}}\Delta a_{i-1}+\frac{\partial O_i}{\partial {\alpha }_{i-1}}\Delta {\alpha }_{i-1}+\frac{\partial O_i}{\partial d_i}\Delta d_i+\frac{\partial O_i}{\partial {\theta }_i}\Delta {\theta }_i$$
把两者组合起来可写作雅可比行列式形式：
$$\Delta X_i=[\begin{array}{c}d{P}_i\\ {\delta }_i\end{array}]={\Psi }_i\Delta {\xi }_i$$
式中，$d{P}_i=[d{x}_i,d{y}_i,d{z}_i{]}^T$，$\Delta {\xi }_i=[\Delta a_{i-1},\Delta {\alpha }_{i-1},\Delta d_i,\Delta {\theta }_i{]}^T$，${\delta }_i={\left[\begin{array}{c}\delta x_i,\delta y_i,\delta z_i\end{array}\right]}^T$，${\Psi }_i$是一个6*4的矩阵

1.1 末端位置全微分

由于位置变换是平移变换，所以总的位置误差可以由各关节误差累加得到：
$$d{P}_n=\left[\begin{array}{c}dx\\ dy\\ dz\end{array}\right]=\left[\begin{array}{cccc}{M}_a& M_{\alpha }& M_d& M_{\theta }\end{array}\right]\left[\begin{array}{c}\Delta a\\ \Delta \alpha \\ \Delta d\\ \Delta \theta \end{array}\right]\text{\hspace{1em}(2)}$$
$d{P}_n$表示机器人末端位置的偏差值，$M_a,M_{\alpha },M_d,M_{\theta }$为3*n矩阵，是机器人的运动学参数偏差的系数矩阵，对应着上述对应下标为i时雅可比矩阵的前三行，$\Delta a,\Delta \alpha ,\Delta d,\Delta \theta$时机器人运动学的参数偏差，其中$\Delta a=[\Delta a_1,\Delta a_2,...,\Delta a_n{]}^T$，其他三个参数同理。

1.2 末端姿态全微分

$${\delta }_n=\left[\begin{array}{c}\delta x\\ \delta y\\ \delta z\end{array}\right]=\left[\begin{array}{cccc}{M}_a& M_{\alpha }& M_d& M_{\theta }\end{array}\right]\left[\begin{array}{c}\Delta a\\ \Delta \alpha \\ \Delta d\\ \Delta \theta \end{array}\right]\text{\hspace{1em}(3)}$$
${\delta }_n$表示机器人末端姿态的偏差值，$M_a,M_{\alpha },M_d,M_{\theta }$为3*n矩阵，是机器人的运动学参数偏差的系数矩阵，对应着上述对应下标为i时雅可比矩阵的后三行，$\Delta a,\Delta \alpha ,\Delta d,\Delta \theta$时机器人运动学的参数偏差，其中$\Delta a=[\Delta a_1,\Delta a_2,...,\Delta a_n{]}^T$，其他三个参数同理。

1.3 末端位姿全微分

结合前面所述，位姿全微分可写为：
$$\Delta X_n=[\begin{array}{c}d{P}_n\\ {\delta }_n\end{array}]=\Psi \Delta \xi \text{\hspace{1em}(4)}$$
其中
- $d{P}_n$：(3x1)来自$T_n$第四列的位置偏差
- ${\delta }_n$：(3x1)来自旋转矩阵$R_n$的姿态偏差
- $\Psi$：(6x(nx4))广义雅可比矩阵
- $\Delta \xi$：(nx4)n个关节的几何参数偏差

2 机器人运动学参数辨识算法

2.1 偏差辨识流程

流程如下：

2.2 最小二乘法

以位置误差为例：多组位置误差数据构建的几何参数误差模型是超定方程，公式(4)的微分几何参数误差模型实际上是一个线性方程组，包含3个线性方程，方程中的位置误差$\Delta P_n$可以通过测量直接得到，nx4个几何误差是未知数，需要对多个位置进行测量构建超定方程组，从而求几何参数误差的近似解，以六轴机器人为例，共有24个几何误差未知数，而一个点的位置误差可以增加三个求解条件（即x,y,z），所以至少需要8组位置误差数据才能求解，而实际中应采集的点数应该大于这个数。
公式(4)可以写成$Ax=b$的形式，辨识机器人运动学参数的具体误差就是求出$x$的值。如果矩阵 A 是可逆的，可以直接计算出 x 的值：$x=A^{-1}\cdot b$。但是为了辨识机器人的运动学参数偏差，矩阵 A 的行数（采集点的数量*3）总是会大于列数（需要辨识的参数），也就是待求解参数的数量小于方程的数量，不能通过矩阵的逆运算求取机器人的运动学参数偏差，则需要通过最小二乘法来求取。

最小二乘法的具体过程如下：


结合(4)式，这里用最小二乘法求得的运动学参数偏差可以写为如下形式：
$$\Delta \xi =({\Psi }^T\Psi {)}^{-1}{\Psi }^T\Delta \hat{P}$$
由于最小二乘法只进行一次计算，所以使用该方法进行参数辨识时可以和迭代 方法结合，从而减少位置误差，提高参数辨识效果

2.3 机器人定位误差补偿

根据辨识得 到的机器人运动学参数偏差值和机器人运动到期望位姿的各个关节的理论关节角，可以 计算出机器人在该目标位置的位姿偏差 $\Delta T$ ，使用期望位姿减去该位姿偏差就可以得到 补偿后的位姿，将 补偿后的位姿赋予机器人控制器可使机器人运动到期望位置，笛卡尔空间补偿法，如图所示：

3 参考文献

[1] 闻了然和张方, 《机器人运动学模型参数修正》, 数字技术与应用, 卷 41, 期 4, 页 1-4+29, 2023.
[2] 潘钊, 《基于机器学习的工业机器人位置误差标定研究》, 华中科技大学, 2022.
[3] 郭哲, 《垂直六关节工业机器人运动学参数标定》, 东南大学, 2019.