CN112025772A - 一种基于视觉测量的机械臂自主标定方法 - Google Patents

Abstract

本发明公开了一种基于视觉测量的机械臂自主标定方法，包括以下步骤：步骤一：构建非稳态多传感器机械臂视觉系统；步骤二：根据高斯运动方法建立标定模型；步骤三：根据扩展卡尔曼滤波测量多标识点；步骤四：根据线性二次型调节器修正运动轨迹；步骤五：基于蒙特卡罗方法筛选最优测量点；步骤六：采用DH增广模型逼近模型参数。本发明中，通过EKF算法滤波过程、LQR方法修正过程和基于误差分布的标定数据择优筛选过程，可以在机械臂运动误差和测量误差同时存在的情况下获得最优的标定精度；并且本发明成本低、效率高，且较为便捷，可以广泛应用于机械臂类设备长期、频繁的标定维护中。

Description

一种基于视觉测量的机械臂自主标定方法

技术领域

本发明涉及一种基于视觉测量的机械臂自主标定方法。

背景技术

机器人定位精度的标定问题一直是业界关注焦点，现有研究表明 90％的定位误差来源于机构的几何模型误差。机器人机械结构受系统 振动、过载、碰撞和磨损影响，定位误差会不断增大。应用激光跟踪 器等精密测量工具和离线标定算法，可以提高机械臂的定位精度。然 而在航空航天、核工业、深海等特殊环境下，无人操控自主机械臂定 位精度的提高仍是尚在困扰业界的技术难题，主要出于两方面原因。 首先，待标定机械臂的运动势必存在一定误差，为了保证自主机械臂 在标定运动中的安全性，需要使机械臂最大程度地跟踪预定义轨迹。 其次，视觉系统常用于自主标定方法，但其测量误差极大地限制了标 定精度，如何提高测量精度，选择最优测量点成为标定的关键。视觉 系统相当于机器人的“眼睛”，机械臂末端相当于机器人的“手”, 通过手眼之间的配合完成设置的动作任务。机器人运动学参数的标定 大多基于DH方法，因为这种方法需要的参数最少，但奇异点问题是 其主要缺点。

现有技术中，标定过程一般需要用到激光跟踪仪等外部高精度传 感器,事实上，标定精度还会受到视觉相机像素和误差水平的影响。

发明内容

本发明的目的是提供一种基于视觉测量的机械臂自主标定方法， 解决了现有技术中待标定机械臂的运动存在一定温差的问题。

为了实现上述目的，本发明采用了如下技术方案：

一种基于视觉测量的机械臂自主标定方法，包括以下步骤：

步骤一：构建非稳态多传感器机械臂视觉系统；

步骤二：根据高斯运动方法建立标定模型；

步骤三：根据扩展卡尔曼滤波测量多标识点；

步骤四：根据线性二次型调节器修正运动轨迹；

步骤五：基于蒙特卡罗方法筛选最优测量点；

步骤六：采用DH增广模型逼近模型参数。

作为上述技术方案的进一步描述：

步骤一包括如下步骤：

1)以中国空间站生命科学手套箱机械臂为研究平台，建立改进 DH模型，在DH表中添加一组角度，即连杆绕y轴旋转的误差角；

2)机械臂末端的测量系统基于三个空间位置已知的二维码标识 和一个相机模组，机械臂可以使用该相机自动识别二维码标识以形成 一种多源信息反馈，将机械臂的基坐标{C_b}设为整个系统的基坐标， 基坐标系下三个二维代码标识的位姿为

已通过离线测量 确定，当标识点处于摄像机视野中时，终点的实际位置

是相机测量坐标系下实时获得的标识点位置；当相机同时检 测到视场内多个标识点，得到具有一定误差范围的多个测量值；视觉 系统的测量是非稳态的，

呈现出时变的、有界的误差特性。

作为上述技术方案的进一步描述：

步骤二包括如下步骤：

假设

是规划得到的轨迹，x(t)∈Rⁿ和u(t)∈Rⁿ是各个控制周期的目的位置及其输入指令，根据离散系统控制原理， 第(t+1)步中的状态可以写成

x(t+1)＝A(t)x(t)+B(t)u(t)+C(t)w(t)#(2)

其中，w(t)是白噪声，A(t)、B(t)和C(t)是具有兼容尺度的时变 矩阵；

对于本文平台的手眼测量系统，测量值和系统状态之间的关系可 以写为

y_i(t)＝H_i(t)x(t)+v_i(t)，i＝1，2，3#(3)

其中，y_i(t)通过第i个标识测量测量得到的机械臂末端位置，H_i(t) 是机械臂状态空间与测量坐标空间的映射矩阵。

作为上述技术方案的进一步描述：

步骤三包括如下步骤：

根据局部最优卡尔曼滤波器，在第(t+1)步中使用标记的反馈值 获得机械手末端的最优位置

第(t+1)步的先验估计是

K_i是卡尔曼增益，ε_i是测 量值与原理值之差，假设w(t)和v_i(t)都是白噪声；

δ_j，k是克罗内克积增量函数，而j，k分别表征其行和列，先验 估计为

其中，

那么测量值和理论值之间的误差为

卡尔曼增益可计算为

P_i(t+1|t)是先验估计协方差，最优估计协方差是P_i(t+1|t+1)。

P_i(t+1|t+1)＝[I_n-K_i(t+1)H_i(t+1)]·P_i(t+1|t)#(10)

利用方程(4)～(10)，可以估计得到机械臂的最优末端位置，实时 地，当视觉相机捕获I个标记时(I＝1，2，3)，测量系统计算出I个互 异的最优位置

那么最终的测量值可以用矩阵权重来计算

其中最佳矩阵权重Δ_i，i＝1，2，3计算如下

Δ＝∧^-1e(e^T∧^-1e)^-1#(12)

Δ＝[Δ₁，Δ₂，Δ₃]^T，e＝[I_n，I_n，I_n]^T and∧＝(P_ij)_3n×3n， (i，j＝1，2，3)是正定矩阵。

作为上述技术方案的进一步描述：

步骤四包括如下步骤：

假设预先规划的自动运动轨迹

当机械臂存在结构误差时，实际运动轨迹会偏离规划轨迹，有与 障碍物发生干涉碰撞的可能，因此，为了安全连续的自动运动，需要 实时修正其运动轨迹，修改过程是典型的LQR控制，当最优目标写为

E＝α·I

F＝β·I

式(13)的解是控制输入的修正，其解可以写为

S_l＝E

作为上述技术方案的进一步描述：

步骤五包括如下步骤：

据卡尔曼滤波，位置估计更精确的测量点对应的协方差更小，从 数学角度，协方差以矩阵表达，从几何角度，协方差是一个空间椭球， 对最优估计协方差P_i(t+1|t+1)进行特征分解，得到特征值D_i和特征 向量V_i，

[D_i,V_i]＝Eig(P_i(t+1|t+1))#(15)

特征值D_i是椭球体的轴长，选择最小协方差的评估标准

利用式(16)的计算结果作为评估标准，选择方差椭球最小的测量 点标定机械臂，被选择的点集Ψ可表示为

Ψ＝[x_b,0,…,x_b,k,…x_b,m](0≤k≤m)#(17)。

作为上述技术方案的进一步描述：

步骤六包括如下步骤：

为了避免从测量坐标到机械臂基坐标的变换，从而减少测量误差 累积，提出了一种基于距离误差的运动学标定模型，以修正扩展DH 参数，避免复杂的坐标变换，简化测量过程，距离误差

Δl_k,t＝|x_c,k-x_c,t|-|x_b,k-x_b,t|(0≤k,t≤m)#(18)

其中，x_c,k,x_c,t,x_b,k,x_b,t是机械臂末端位置矢量，x_c,k，x_c,t是测 量坐标系中的点矢量，x_b,k,x_b,t是机械臂基坐标中的位置矢量；式(18) 也可以写成

其中，dx是位置误差的矢量，根据扩展DH参数，关节的变换矩 阵可以写成

T_i＝R(α_i-1)T(a_i-1)T(θ_i)R(d_i)R(β_i)#(20)

对于单个关节，关节偏差是所有误差项的叠加；

末端执行器的姿态矩阵及其误差矩阵

展开式(16)，省略二阶微分项

在式(19)中，根据李代数，齐次矩阵的偏差应该满足式(21)的形 式

式(20)与式(21)相对应，方程集合

其中：

dx＝[d_x d_y d_z]^T，

将式(22)代入式(16)

由于

和

很小，可以 视为相等的，因此可写成

x_b,k和x_b,t是由式(10)估计的样本集合中的第k个和第t个点的最 优估计。

对增广DH参数

的标定可以通过求解方程(26)来计算。新的DH 参数可以通过将

累加到最后的参数值中进行校正

本发明至少具备以下有益效果：

通过EKF算法滤波过程、LQR方法修正过程和基于误差分布的标 定数据择优筛选过程，可以在机械臂运动误差和测量误差同时存在的 情况下获得最优的标定精度；并且本发明成本低、效率高，且较为便 捷，可以广泛应用于机械臂类设备长期、频繁的标定维护中。

附图说明

为了更清楚地说明本发明实施例技术方案，下面将对实施例描述 中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图 是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创 造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了根据本发明实施例提供的机械臂测量系统图；

图2示出了根据本发明实施例提供的机械臂运动学坐标图；

图3示出了根据本发明实施例提供的稳态EKF流程图；

图4示出了根据本发明实施例提供的校准算法流程图；

图5示出了根据本发明实施例提供的全工作空间的标定结果图；

图6示出了根据本发明实施例提供的全工作空间的标定精度及 误差分布图；

图7示出了根据本发明实施例提供的公共工作空间中的校准结 果图；

图8示出了根据本发明实施例提供的公共工作空间中的校准精 度及误差分布图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合 附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描 述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例一

参照图1-8，一种基于视觉测量的机械臂自主标定方法，包括以 下步骤：

步骤一：构建非稳态多传感器机械臂视觉系统；

以中国空间站生命科学手套箱机械臂为研究平台，为了获得较高 的定位精度，提高模型与实际机械臂几何构型的契合程度，建立了更 严格的改进DH模型，在DH表中添加了一组角度，以表示连杆绕y轴 旋转的误差角，优选的，所有角度值都为零；轴1是圆弧导轨，在机 械模型中，它被抽象为带有虚拟链接的旋转轴，该连杆长度等于圆弧 导轨的半径，准备过程中测得三个二维码标识点在基坐标系中位置分 别位于(-60.69，-69.52，61.23)，(-61.81，50.24，61.02)和 (61.16，-68.55，61.12)。

表1 DH参数表

机械臂末端的测量系统如图1所示，基于三个空间位置已知的二 维码标识和一个相机模组，机械臂可以使用该相机自动识别二维码标 识以形成一种多源信息反馈，将机械臂的基坐标{C_b}设为整个系统的 基坐标，基坐标系下三个二维代码标识的位姿为

已通过 离线测量确定，当标识点处于摄像机视野中时，终点的实际位置

是相机测量坐标系下实时获得的标识点位置，依据设计调试 过程中的相机参数和实验，视觉系统的位置和姿态测量误差的方差为 [0.1；0.1；0.3mm；0.2°；0.2°；1.0°]²。

步骤二：根据高斯运动方法建立标定模型；

构建如图2所示的机械臂运动学模型，假设

是规 划得到的轨迹，x(t)∈Rⁿ和u(t)∈Rⁿ是各个控制周期的目的位置及 其输入指令，根据离散系统控制原理，第(t+1)步中的状态可以写成

x(t+1)＝A(t)x(t)+B(t)u(t)+C(t)w(t)#(2)

其中，w(t)是白噪声，A(t)、B(t)和C(t)是具有兼容尺度的时变 矩阵，对于本文平台的手眼测量系统，测量值和系统状态之间的关系 可以写为

y_i(t)＝H_i(t)x(t)+v_i(t)，i＝1,2,3#(3)

其中，y_i(t)通过第i个标识测量测量得到的机械臂末端位置，H_i(t) 是机械臂状态空间与测量坐标空间的映射矩阵。

步骤三：根据扩展卡尔曼滤波测量多标识点；

稳态EKF多标识测量的流程图如图3所示，整个数据融合过程分 两步完成，第一步是在判断标识点测量有效后，通过EKF方法实现理 论位置和测量位置的数据融合，获得单个标识点的最优估计，第二步 是将1-3个最优估计进行数据融合，得到最终的测量值；

根据局部最优卡尔曼滤波器，在第(t+1)步中使用标记的反馈值 获得机械手末端的最优位置

第(t+1)步的先验估计是

K_i是卡尔曼增益，ε_i是测 量值与原理值之差，假设w(t)和v_i(t)都是白噪声；

δ_j,k是克罗内克积增量函数，而j，k分别表征其行和列，先验 估计为

其中，

那么测量值和理论值之间的误差为

卡尔曼增益可计算为

P_i(t+1|t)是先验估计协方差，最优估计协方差是P_i(t+1|t+1)。

P_i(t+1|t+1)＝[I_n-K_i(t+1)H_i(t+1)]·P_i(t+1|t)#(10)

利用方程(4)-(10)，可以估计得到机械臂的最优末端位置，实时 地，当视觉相机捕获I个标记时(I＝1,2,3)，测量系统计算出I个互 异的最优位置

那么最终的测量值可以用矩阵权重来计算

其中最佳矩阵权重Δ_i,i＝1,2,3计算如下

Δ＝Λ^-1e(e^TΛ^-1e)^-1#(12)

Δ＝[Δ₁,Δ₂,Δ₃]^T,e＝[I_n,I_n,I_n]^T andΛ＝(P_ij)_3n×3n, (i,j＝1,2,3)是正定矩阵。

步骤四：根据线性二次型调节器修正运动轨迹；

假设预先规划的自动运动轨迹

当机械臂存在结构误差时，实际运动轨迹会偏离规划轨迹，有与 障碍物发生干涉碰撞的可能，因此，为了安全连续的自动运动，需要 实时修正其运动轨迹，修改过程是典型的LQR控制，当最优目标写为

E＝α·I

F＝β·I

式(13)的解是控制输入的修正，其解可以写为

S_l＝E

步骤五：基于蒙特卡罗方法筛选最优测量点；

据卡尔曼滤波，位置估计更精确的测量点对应的协方差更小，从 数学角度，协方差以矩阵表达，从几何角度，协方差是一个空间椭球， 对最优估计协方差P_i(t+1|t+1)进行特征分解，得到特征值D_i和特征 向量V_i；

[D_i,V_i]＝Eig(P_i(t+1|t+1))#(15)

特征值D_i是椭球体的轴长，选择最小协方差的评估标准

利用式(16)的计算结果作为评估标准，选择方差椭球最小的测量 点标定机械臂，被选择的点集Ψ可表示为

Ψ＝[x_b,0,…,x_b,k,…x_b,m](0≤k≤m)#(17)。

步骤六：采用DH增广模型逼近模型参数。

为了避免从测量坐标到机械臂基坐标的变换，从而减少测量误差 累积，提出了一种基于距离误差的运动学标定模型，以修正扩展DH 参数，避免复杂的坐标变换，简化测量过程，距离误差

Δl_k,t＝|x_c,k-x_c,t|-|x_b,k-x_b,t|(0≤k,t≤m)#(18)

其中，x_c,k,x_c,t,x_b,k,x_b,t是机械臂末端位置矢量，x_c,k，x_c,t是测 量坐标系中的点矢量，x_b,k,x_b,t是机械臂基坐标中的位置矢量，式(18) 也可以写成

其中，dx是位置误差的矢量，根据表1中扩展DH参数，关节的 变换矩阵可以写成

T_i＝R(α_i-1)T(a_i-1)T(θ_i)R(d_i)R(β_i)#(20)

对于单个关节，关节偏差是所有误差项的叠加，

末端执行器的姿态矩阵及其误差矩阵

展开式(16)，省略二阶微分项

在式(19)中，根据李代数，齐次矩阵的偏差应该满足式(21)的形 式

式(20)与式(21)相对应，方程集合

其中：

dx＝[d_x d_y d_z]^T，

将式(22)代入式(16)

由于

和

很小，可以 视为相等的，因此可写成

x_b,k和x_b,t是由式(10)估计的样本集合中的第k个和第t个点的最 优估计，

对增广DH参数

的标定可以通过求解方程(26)来计算，新的DH 参数可以通过将

累加到最后的参数值中进行校正，整个校准算法 流程图如图4所示；

基于快速随机扩展树算法(RRT)，提前规划了避障轨迹，随着机 械臂沿着轨迹移动，末端手眼相机可以捕捉到0-3个二维码标记，由 于光的反射及其视场范围的限制，常常无法同时测得三个标识点，但 基于步骤二中提出的多标记稳态EKF位置估计方法，都可以获得一个 位置的最优估计，并且同步得到表征误差分布特性的协方差矩阵，选 择协方差最小的一百个点，随机两两组合形成距离标定信息Δl_k,t，依 据式(24)进行标定计算，为了验证本文提出的自主视觉标定方法的 有效性和实用性，使用高精度跟踪器测量一组点，针对实验平台，用 这些点分别基于扩展DH参数和传统DH参数进行标定，并将其结果与 本文自动视觉标定方法的结果进行比较，这三种方法的标定结果如表 2所示；基于得到的三组DH参数标定结果，对标定误差及其相应的 误差范围进行分析和比较，实验分两个步骤进行，第一步是分析在整 个工作空间随机位置的标定精度特性，而第二个步骤是比较特定局部 工作区内的标定精度。

表2不同标定方法结果比较

为了验证本文算法的精度和有效性，随机选取了分布在整个工作 空间中的20个机械臂位置，测量位置与通过三种标定参数的末端位 置进行比较，误差及其分布如图5和图6所示。从5可以看出，传统 的DH参数标定方法得到的位置精度低于由激光跟踪仪和本文自主视 觉辅助方法的标定精度；图6显示了具体的位置精度及其误差分布； 该方法的定位精度约为0.05mm，略低于激光跟踪仪0.035mm精度； 由于机械臂的实际工作空间受到手套箱的约束，不是其完全工作空 间，因此，需要进一步验证机械臂在特定局部工作空间中的位置精度， 通常有效的工作空间是手套箱中显微镜的两侧，在其有效工作空间中 收集了20个位置点来验证位置的准确性，图7是的20个点的误差结 果非常接近图5所示的标定结果，但是从图8中可以看出，有效减小 了自主视觉辅助标定后的位置误差和激光跟踪仪标定的位置误差。

以上显示和描述了本发明的基本原理、主要特征和本发明的优 点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上 述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神 和范围的前提下本发明还会有各种变化和改进，这些变化和改进都落 入要求保护的本发明的范围内。本发明要求的保护范围由所附的权利 要求书及其等同物界定。

Claims (7)

1.一种基于视觉测量的机械臂自主标定方法，其特征在于：包括以下步骤：

步骤一：构建非稳态多传感器机械臂视觉系统；

步骤二：根据高斯运动方法建立标定模型；

步骤三：根据扩展卡尔曼滤波测量多标识点；

步骤四：根据线性二次型调节器修正运动轨迹；

步骤五：基于蒙特卡罗方法筛选最优测量点；

步骤六：采用DH增广模型逼近模型参数。

2.根据权利要求1所述的一种基于视觉测量的机械臂自主标定方法，其特征在于，步骤一包括如下步骤：

1)以中国空间站生命科学手套箱机械臂为研究平台，建立改进DH模型，在DH表中添加一组角度，即连杆绕y轴旋转的误差角；

2)机械臂末端的测量系统基于三个空间位置已知的二维码标识和一个相机模组，机械臂可以使用该相机自动识别二维码标识以形成一种多源信息反馈，将机械臂的基坐标{C_b}设为整个系统的基坐标，基坐标系下三个二维代码标识的位姿为

已通过离线测量确定，当标识点处于摄像机视野中时，终点的实际位置

是相机测量坐标系下实时获得的标识点位置；当相机同时检测到视场内多个标识点，得到具有一定误差范围的多个测量值；视觉系统的测量是非稳态的，

呈现出时变的、有界的误差特性。

3.根据权利要求1所述的一种基于视觉测量的机械臂自主标定方法，其特征在于，步骤二包括如下步骤：

假设Γ＝{x(t)，u(t)}是规划得到的轨迹，x(t)∈Rⁿ和u(t)∈Rⁿ是各个控制周期的目的位置及其输入指令，根据离散系统控制原理，第(t+1)步中的状态可以写成

x(t+1)＝A(t)x(t)+B(t)u(t)+C(t)w(t)#(2)

其中，w(t)是白噪声，A(t)、B(t)和C(t)是具有兼容尺度的时变矩阵；

对于本文平台的手眼测量系统，测量值和系统状态之间的关系可以写为

y_i(t)＝H_i(t)x(t)+v_i(t)，i＝1，2，3#(3)

其中，y_i(t)通过第i个标识测量测量得到的机械臂末端位置，H_i(t)是机械臂状态空间与测量坐标空间的映射矩阵。

4.根据权利要求1所述的一种基于视觉测量的机械臂自主标定方法，其特征在于，步骤三包括如下步骤：

根据局部最优卡尔曼滤波器，在第(t+1)步中使用标记的反馈值获得机械手末端的最优位置

第(t+1)步的先验估计是

K_i是卡尔曼增益，ε_i是测量值与原理值之差，假设w(t)和v_i(t)都是白噪声；

δ_j，k是克罗内克积增量函数，而j，k分别表征其行和列，先验估计为

其中，

那么测量值和理论值之间的误差为

卡尔曼增益可计算为

P_i(t+1|t)是先验估计协方差，最优估计协方差是P_i(t+1|t+1)

P_i(t+1|t+1)＝[I_n-K_i(t+1)H_i(t+1)]·P_i(t+1|t)#(10)

利用方程(4)～(10)，可以估计得到机械臂的最优末端位置，实时地，当视觉相机捕获I个标记时(I＝1,2,3)，测量系统计算出I个互异的最优位置

那么最终的测量值可以用矩阵权重来计算

其中最佳矩阵权重Δ_i,i＝1,2,3计算如下

Δ＝Λ^-1e(e^TΛ^-1e)^-1#(12)

Δ＝[Δ₁,Δ₂,Δ₃]^T,e＝[I_n,I_n,I_n]^T and Λ＝(P_ij)_3n×3n,(i,j＝1,2,3)是正定矩阵。

5.根据权利要求1所述的一种基于视觉测量的机械臂自主标定方法，其特征在于，步骤四包括如下步骤：

假设预先规划的自动运动轨迹

Γ＝{(x₁,u₁),(x₂,u₂),…9x_t,u_t),…,(x_m,u_m)}

当机械臂存在结构误差时，实际运动轨迹会偏离规划轨迹，有与障碍物发生干涉碰撞的可能，因此，为了安全连续的自动运动，需要实时修正其运动轨迹，修改过程是典型的LQR控制，当最优目标写为

E＝α·I

F＝β·I

式(13)的解是控制输入的修正，其解可以写为

S_l＝E

6.根据权利要求1所述的一种基于视觉测量的机械臂自主标定方法，其特征在于，步骤五包括如下步骤：

据卡尔曼滤波，位置估计更精确的测量点对应的协方差更小，从数学角度，协方差以矩阵表达，从几何角度，协方差是一个空间椭球，对最优估计协方差P_i(t+1|t+1)进行特征分解，得到特征值D_i和特征向量V_i，

[D_i,V_i]＝Eig(P_i(t+1|t+1))#(15)

特征值D_i是椭球体的轴长，选择最小协方差的评估标准

利用式(16)的计算结果作为评估标准，选择方差椭球最小的测量点标定机械臂，被选择的点集Ψ可表示为

Ψ＝[x_b,0,…,x_b,k,…x_b,m](0≤k≤m)#(17)。

7.根据权利要求1所述的一种基于视觉测量的机械臂自主标定方法，其特征在于，步骤六包括如下步骤：

为了避免从测量坐标到机械臂基坐标的变换，从而减少测量误差累积，提出了一种基于距离误差的运动学标定模型，以修正扩展DH参数，避免复杂的坐标变换，简化测量过程，距离误差

Δl_k,t＝|x_c,k-x_c,t|-|x_b,k-x_b,t|(0≤k,t≤m)#(18)

其中，x_c,k,x_c,t,x_b,k,x_b,t是机械臂末端位置矢量，x_c,k，x_c,t是测量坐标系中的点矢量，x_b,k,x_b,t是机械臂基坐标中的位置矢量；式(18)也可以写成

其中，dx是位置误差的矢量，根据扩展DH参数，关节的变换矩阵可以写成

T_i＝R(α_i-1)T(a_i-1)T(θ_i)R(d_i)R(β_i)#(20)

对于单个关节，关节偏差是所有误差项的叠加；

末端执行器的姿态矩阵及其误差矩阵

展开式(16)，省略二阶微分项

在式(19)中，根据李代数，齐次矩阵的偏差应该满足式(21)的形式

式(20)与式(21)相对应，方程集合

其中：

dx＝[d_x d_y d_z]^T，

将式(22)代入式(16)

由于

和

很小，可以视为相等的，因此可写成

x_b,k和x_b,t是由式(10)估计的样本集合中的第k个和第t个点的最优估计

对增广DH参数

的标定可以通过求解方程(26)来计算，新的DH参数可以通过将

累加到最后的参数值中进行校正

Images