本发明公开了一种面向工业机器人的几何误差辨识方法，包括如下步骤：S1：建立机器人运动学几何误差模型；S2：获取机器人的测量位姿；S3：建立测量位姿综合层次评价指标，综合考虑位姿观测性指标、位姿分布离散性指标和均匀性指标，得到最优测量位姿解集，基于改进的序列浮动前向选择算法，从最优测量位姿解集中，确定最优测量位姿；S4：计算得到不同关节角参数对应的理论位姿；S5：实际测量位姿与理论位姿之差为机器人末端定位误差，根据已建立的机器人运动学几何误差模型，基于自适应临界值的抗差估计的方法进行辨识，迭代进行几何误差辨识，得到最终的几何参数误差。
1.一种面向工业机器人的几何误差辨识方法，其特征在于包括如下步骤：S1：建立机器人运动学几何误差模型：ΔX＝A·ΔQ，其中ΔX为机器人末端定位误差，ΔQ为机器人几何参数误差，A为描述定位误差与机器人参数误差相互关系的系数矩阵；
S2：控制机器人在空间运动，测量机器人末端位置坐标，同时获取机器人关节角参数，得到机器人的测量位姿；
S3：建立测量位姿综合层次评价指标Oc，综合考虑位姿观测性评价指标O1、位姿分布离散性指标δ和均匀性指标U，得到最优测量位姿解集S2，基于改进的序列浮动前向选择算法，从最优测量位姿解集S2中，确定最优测量位姿，算法包括如下步骤：S31：初始化空的最优测量位姿解集ω，初始化算法参数包括：待选位姿集合φ，从待选位姿集合φ中随机选择l个位姿作为初始解，开始算法迭代工作；
S32：序列前向添加，从给定的待选位姿集合φ中确定一个位姿，在添加该位姿后，使得最优测量位姿解集ω的观测性评价指标O1最大；
S33：浮动条件删除，在最优解集ω中判断并删除不重要位姿，直到删除一个不重要位姿后观测性评价指标O1小于加入新位姿之前；
S34：随机交换与删除，采用测量位姿综合层次评价指标Oc作为判断准则，在每一次迭代结束之前，依次从最优测量位姿解集ω中随机选择k个位姿，随机执行与待选位姿解集交换或者删除，根据测量位姿综合层次评价指标Oc判断是否执行交换或者删除；
S35：循环迭代，判断迭代次数是否满足迭代要求，退出循环，得到最优测量位姿Pt；
S4：利用机器人名义MD‑H值，计算得到不同关节角参数对应的理论位姿Pr；
S5：实际测量位姿Pt与理论位姿Pr之差为机器人末端定位误差ΔX，根据已建立的机器人运动学几何误差模型，基于自适应临界值的抗差估计的方法进行辨识，迭代进行几何误差辨识，得到最终的几何参数误差ΔQ。
2.根据权利要求1所述的一种面向工业机器人的几何误差辨识方法，其特征在于所述步骤
S11：根据机器人运动学MD‑H模型，通过模型中的几何参数：关节转角θi、关节距离di、关节扭角αi以及绕对应坐标系y轴的旋转角度βi，得到机器人相邻关节的齐次变换矩阵：其中S和C分别代表正弦函数sin和余弦函数cos，βi表示绕对应坐标系y轴的旋转角度，i表示第i个关节；
S12：根据机器人MD‑H参数和相邻坐标系之间的齐次变换矩阵，得到机器人末端与基坐标系的关系：
其中n、o、a、p四个分量分别表示机器人末端位姿中的法向矢量、方位矢量、接近矢量和位置矢量，下标x、y、z表示各个矢量在x、y、z轴上的投影分量；
S13：根据MD‑H模型，将机器人连杆几何参数误差分别标记为：关节转角误差Δθ、关节扭角误差Δα、连杆长度误差Δa、关节距离误差Δd与旋转角误差Δβ，在几何参数误差的影响下，齐次变换矩阵 新增加微分齐次摄动矩阵其中， 表示机器人第i个关节相对于第i‑1个关节的其次变换矩阵；
S14：考虑机器人每一个关节的几何参数误差，机器人末端位姿表示为：其中，N表示关节数， 表示机器人基座标到机器人末端坐标系的变换矩阵， 为的微分，表示齐次变量矩阵的误差；
S15：考虑机器人基坐标系和测量坐标系之间的误差，得到机器人几何误差辨识模型：ΔX＝A·ΔQ
其中ΔX为末端定位误差，即S14中的 A为描述参数误差与定位误差相互关系的系数矩阵，ΔQ为参数误差。
3.根据权利要求1所述的一种面向工业机器人的几何误差辨识方法，其特征在于所述步骤S3中，误差辨识模型系数矩阵A为m×n阶矩阵，系数矩阵奇异值σ1，σ2...σn，得到位姿观测性评价指标：
测量位姿的空间分布，包括位姿分布的离散性评价指标δ和均匀性评价指标U；
测量范围S中的关节角集合为C＝{θ1，θ2，...θn}，θi对应末端位置为pi(θi)，得到测量位姿分布的离散性评价指标为：
i、j∈n，分别表示第i、第j个关节角，且i≠j；
测量位姿分布的均匀性评价指标为：其中 表示测量点与中心点的平均距离，d(y，pk)表示y与pk之间的欧氏距离，pk表示第k组测量位姿；
利用反正切函数的非线性归一化，引入加权控制因子λ1、λ2、λ3，得到位姿综合评价指标：
选择O1作为一级指标，根据O1选择测量位姿得到集合S1，以测量位姿综合层次评价指标Oc作为二级指标，再对集合S1进行优化，通过权值控制δ、U和O1，从集合S1中按照测量位姿综合层次评价指标Oc选择得到最优测量位姿的解集S2。
4.根据权利要求1所述的一种面向工业机器人的几何误差辨识方法，其特征在于所述步骤
S5中，自适应临界值抗差估计的方法，通过修改最小二乘法的权值矩阵，在迭代拟合过程中不断减小误差较大的观测值对应的权值矩阵，包括如下步骤：S51：构建抗差估计最小二乘法的权值矩阵P，具体形式为：采用加权最小二乘法的权值矩阵进行几何参数误差的辨识；
S52：根据参数误差辨识的结果计算残差分布状态，并基于自适应构造临界值的IGG3权值函数，调整权值矩阵，再次迭代，直到达到辨识精度的阈值要求或者迭代次数，退出迭代，完成参数误差辨识。
5.根据权利要求4所述的一种面向工业机器人的几何误差辨识方法，其特征在于所述步骤S52中，在最小二乘法的权值矩阵迭代计算过程中，权值矩阵通过IGG3权值函数确定，根据每次迭代结果的残差分布状态计算指定置信水平的置信区间，权值函数的临界值根据置信区间的极限值来自适应构造；
其中，k0、k1为调和系数， 为标准化后的残差，为下限值，为上限值，对低于置信区间下限的观测值的权值进行保留，对位于区间内的观测值进行降权，大于上限值的观测值直接舍弃。
6.根据权利要求5所述的一种面向工业机器人的几何误差辨识方法，其特征在于所述k0取值1.0～1.5，k1取值2.5～3.0。
7.根据权利要求1所述的一种面向工业机器人的几何误差辨识方法，其特征在于所述步骤S32中，将位姿代入位姿评价指标函数为fadd(θ)，当函数值大于观测性评价指标O1时，将该位姿添加到最优测量位姿解集ω中。
8.根据权利要求1所述的一种面向工业机器人的几何误差辨识方法，其特征在于所述步骤S33中，将位姿代入位姿评价指标函数为frmv(θ)，当函数值大于观测性评价指标O1时，将该位姿从最优测量位姿解集ω中删除。
9.根据权利要求1所述的一种面向工业机器人的几何误差辨识方法，其特征在于所述步骤S34中，交换还是删除是随机确定的，将要判断的位姿代入位姿评价指标函数为frand(θ)，当函数值大于观测性评价指标Oc时，则进行交换或删除。
一种面向工业机器人的几何误差辨识方法
技术领域
[0001] 本发明涉及机器人运动学、机器人误差辨识领域，尤其是涉及一种面向工业机器人的几何误差辨识方法。
背景技术
[0002] 随着工业技术的高速发展，工业机器人广泛应用在现代制造业的许多领域，在搬运、喷涂、焊接、装配等众多领域发挥了重要的作用，极大的提高了生产效率，有利的促进了
经济与社会的发展。在一些新兴的行业和领域，例如无人化工厂、太空探索、人机交互协作
等，工业机器人也发挥了重要的作用，工业机器人越来越成为现在制造业不可缺少的重要
角色。
[0003] 在复杂度高或精度要求高的领域，例如柔性制造、磨削、钻孔等领域，对工业机器人定位精度要求高。机器人定位精度分为绝对定位精度和重复定位精度，目前，机器人的重
复定位精度可达0.01mm，基本满足使用要求，影响机器人定位精度的主要是绝对定位精度，
也就是机器人实际位置与目标位置之间的误差。造成这一误差的原因有很多，机械加工过
程中的误差、装配误差、零件使用中的磨损以及机器人工作温度和工作负载的变化等等都
会引起机器人末端定位精度的降低。根据误差源的特点，误差分类可分为几何误差和非几
何误差。其中，几何误差是指机器人运动学描述参数，例如连杆长度、连杆扭角、关节偏置、
关节转角等参数实际值与名义值之间的误差。经过机器人的机械机构，误差在各连杆间进
行误差传递和积累，最终导致机器人定位精度降低。非几何误差包括热效应、连杆刚度变化
导致的变形，相较于几何误差，非几何误差难以量化。就精度问题而言，几何误差是造成机
器人末端定位精度降低的主要因素，针对几何误差进行辨识是提高机器人定位精度的有效
途径。
[0004] 机器人几何误差辨识精度与所选测量位姿关系密切，不同的测量位姿对几何参数误差的反映能力不同。针对测量位姿的评价指标，常用的观测性指标没有考虑位姿的分布
性；综合考虑观测性和位姿分布性的综合评价指标，由于归一化的操作降低了指标的敏感
度。针对测量位姿的选择，广泛使用的选择算法存在局部收敛问题，算法的收敛效果受参数
初始值影响。进行几何误差辨识时，辨识精度也容易受到测量过程测量仪器以及粗差影响
而降低。
发明内容
[0005] 为解决现有技术的不足，实现提高工业机器人几何误差辨识精度的目的，本发明采用如下的技术方案：
[0006] 一种面向工业机器人的几何误差辨识方法，包括如下步骤：
[0007] S1：建立机器人运动学几何误差模型：ΔX＝A·ΔQ，其中ΔX为机器人末端定位误差，ΔQ为机器人几何参数误差，A为描述定位误差与机器人参数误差相互关系的系数矩阵；
[0008] S2：控制机器人在空间运动，利用激光跟踪仪测量机器人末端执行器位置坐标，同时获取机器人关节角参数，得到机器人大量的测量位姿；
[0009] S3：建立测量位姿综合层次评价指标Oc，综合考虑位姿观测性指标O1、位姿分布离散性指标δ和均匀性指标U，得到最优测量位姿解集S2，基于改进的序列浮动前向选择算法，
从最优测量位姿解集S2中，确定最优测量位姿，算法包括如下步骤：
[0010] S31：初始化空的最优测量位姿解集ω，初始化算法参数包括：待选位姿集合φ、需求的位姿数m、迭代次数e、随机交换删除的位姿数量P以及初始解的位姿数量m等参数，从待
选位姿集合φ中随机选择l个位姿作为初始解，开始算法迭代工作；
[0011] S32：序列前向添加，从给定的待选位姿集合φ中确定一个位姿，在添加该位姿后，使得最优测量位姿解集ω的观测性评价指标O1最大；
[0012] S33：浮动条件删除，在最优解集ω中判断并删除不重要位姿，直到删除一个不重要位姿后观测性评价指标O1小于加入新位姿之前；
[0013] S34：随机交换与删除，为增加整个过程的随机性以避免解集陷入局部极值，采用测量位姿综合层次评价指标Oc作为判断准则，通过权值控制弱化系数矩阵的观测性，增加
数据的分布性重要程度，在每一次迭代结束之前，依次从最优测量位姿解集ω中随机选择k
个位姿，随机执行与待选位姿解集交换或者删除，根据测量位姿综合层次评价指标Oc判断
是否执行交换或者删除；
[0014] S35：循环迭代，判断迭代次数是否满足迭代要求，退出循环，得到最优测量位姿Pt；
[0015] S4：利用机器人名义MD‑H值，计算得到不同关节角参数对应的理论位姿Pr；
[0016] S5：实际测量位姿Pt与理论位姿Pr之差为机器人末端定位误差ΔX，根据已建立的机器人运动学几何误差模型，基于自适应临界值的抗差估计的方法进行辨识，消除测量过
程中测量扰动以及仪器本身误差的影响，提高辨识精度，迭代进行几何误差辨识，得到最终
的几何参数误差ΔQ。
[0017] 进一步地，所述步骤S1包括如下步骤：
[0018] S11：根据机器人运动学MD‑H模型，通过模型中的几何参数：关节转角θi、关节距离di、连杆长度ai、关节扭角αi以及旋转角βi，得到机器人相邻关节的齐次变换矩阵：
[0019]
[0020] 其中S和C分别代表正弦函数sin和余弦函数cos,βi表示绕对应坐标系y轴的旋转角度，i表示第i个关节；
[0021] S12：根据机器人MD‑H参数和相邻坐标系之间的齐次变换矩阵，得到机器人末端与基坐标系的关系：
[0022]
[0023] 其中n、o、a三个分量分别代机器人末端位姿中姿态的各个分量；
[0024] S13：由于机器人的加工装配误差、连杆传递误差的存在以及环境因素的影响，机器人存在几何参数误差，根据MD‑H模型，将机器人连杆几何参数误差分别标记为：关节转角
误差Δθ、关节扭角误差Δα、连杆长度误差Δa、关节距离误差Δd与旋转角误差Δβ，在几何
i‑1 i‑1
参数误差的影响下，齐次变换矩阵Ti 新增加微分齐次摄动矩阵dTi ：
[0025]
[0026] S14：考虑机器人每一个关节的几何参数误差，机器人末端位姿表示为：
[0027]
[0028] 其中，N表示关节数；
[0029] S15：考虑机器人基坐标系和测量坐标系之间的误差，得到机器人几何误差辨识模型：
[0030] ΔX＝A·ΔQ
[0031] 其中ΔX为末端定位误差，A为描述参数误差与定位误差相互关系的系数矩阵，ΔQ为参数误差。
[0032] 进一步地，所述步骤S3中，误差辨识模型系数矩阵A为m×n阶矩阵，系数矩阵奇异值σ1,σ2...σn，得到位姿观测性评价指标：
[0033]
[0034] 除了测量位姿的观测性指标之外，还需要考虑测量位姿的空间分布情况，包括位姿的离散性评价指标δ和均匀性评价指标U；
[0035] 测量范围S中的关节角集合为C＝{θ1,θ2,...θn}，θi对应末端位置为p(θi)，得到测量位姿的分布性指标为：
[0036]
[0037] i、j∈n，分别表示第i、第j个关节角，且i≠j；
[0038] 测量位姿的离散性评价指标为：
[0039]
[0040] 其中 表示测量点与中心点的平均距离，d(y,pk)表示y与pk之间的欧氏距离；
[0041] 利用反正切函数的非线性归一化，引入加权控制因子λ1、λ2、λ3，控制评价指标中的每个组成部分的权值，得到位姿综合评价指标：
[0042]
[0043] 通过改变权值，综合考虑位姿的观测性、离散性以及均匀性；如设置O1指标的权值λ1为0则完全不考虑可观测性，只关注数据的分布性，也可以设置λ1较小的权值，避免可观测
性受到较大干扰的情况下优化测量位姿的分布性；
[0044] 由于系数矩阵的可观测性指标对参数辨识结果重要性较大，选择O1作为一级指标，根据O1选择测量位姿得到集合S1，以测量位姿综合层次评价指标Oc作为二级指标，再对
集合S1进行优化，通过权值控制δ、U和O1，从集合S1中按照测量位姿综合层次评价指标Oc选
择得到最优测量位姿的解集S2。
[0045] 进一步地，所述步骤S5中，由于在测量实际位姿时，测量仪器以及测量扰动等因素的影响，造成某些测量数据存在较大的波动，因此利用自适应临界值抗差估计的方法对存
在粗差数据进行参数辨识，通过修改最小二乘法的权值矩阵，在迭代拟合过程中不断减小
误差较大的观测值对应的权值矩阵，抑制粗差，包括如下步骤：
[0046] S51：构建抗差估计最小二乘法的权值矩阵P，具体形式为：
[0047]
[0048] 采用加权最小二乘法的权值矩阵进行几何参数误差的辨识；
[0049] S52：根据参数误差辨识的结果计算残差分布状态，并基于自适应构造临界值的IGG3权值函数，调整权值矩阵，再次迭代，直到达到辨识精度的阈值要求或者迭代次数，退
出迭代，完成参数误差辨识。
[0050] 进一步地，所述步骤S52中，在最小二乘法的权值矩阵迭代计算过程中，权值矩阵通过IGG3权值函数确定，根据每次迭代结果的残差分布状态计算指定置信水平的置信区
间，权值函数的临界值根据置信区间的极限值来自适应构造，消除手动设定临界值的风险；
[0051] 自适应临界值构造的权值函数为：
[0052]
[0053] 其中，k0、k1为调和系数， 为标准化后的残差，为下限值，为上限值，上式表明，对低于置信区间下限的观测值的权值进行保留，对位于区间内的观测值进行降权，大于
上限值的观测值直接舍弃。
[0054] 进一步地，所述k0取值1.0～1.5，k1取值2.5～3.0。
[0055] 进一步地，所述步骤S32中，将位姿代入位姿评价指标函数为fadd(θ)，当函数值大于观测性评价指标O1时，将该位姿添加到最优测量位姿解集ω中。
[0056] 进一步地，所述步骤S33中，将位姿代入位姿评价指标函数为frmv(θ)，当函数值大于观测性评价指标O1时，将该位姿从最优测量位姿解集ω中删除。
[0057] 进一步地，所述步骤S34中，交换还是删除是随机确定的，将要判断的位姿代入位姿评价指标函数为frand(θ)，当函数值大于观测性评价指标Oc时，则进行交换或删除。
[0058] 本发明的优势和有益效果在于：
[0059] 本发明综合考虑了位姿观测性指标和位姿分布性指标，增加整个过程的随机性以避免解集陷入局部极值，通过权值控制弱化系数矩阵的观测性，增加数据的分布性重要程
度，基于自适应临界值的抗差估计的方法进行辨识，消除测量过程中测量扰动以及仪器本
身误差的影响，解决机器人几何误差辨识精度受所选测量位姿以及测量方法影响的问题，
提高几何误差辨识精度。
附图说明
[0060] 图1为本发明的方法流程示意图。
[0061] 图2为本发明中参数辨识前后机器人末端定位误差对比图。
具体实施方式
[0062] 以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。
[0063] 本发明的目的在于提供一种面向工业机器人的几何误差辨识方法，解决机器人几何误差辨识精度受所选测量位姿以及测量方法影响的问题，提高几何误差辨识精度。具流
程如图1所示，包括以下步骤：
[0064] S1：建立机器人运动学几何误差模型；
[0065] 根据机器人运动学MD‑H模型，通过模型中的几何参数(关节转角θi、关节距离di、连杆长度ai、关节扭角αi以及旋转角βi)，得到机器人相邻关节的齐次变换矩阵：
[0066]
[0067] 其中S和C分别代表正弦函数sin和余弦函数cos,βi表示绕对应坐标系y轴的旋转角度。
[0068] 根据机器人MD‑H参数和相邻坐标系之间的齐次变换矩阵，得到机器人末端与基坐标系的关系：
[0069]
[0070] 其中n、o、a三个分量分别代机器人末端位姿中姿态的各个分量。
[0071] 由于机器人的加工装配误差、连杆传递误差的存在以及环境因素的影响，机器人存在几何参数误差。根据MD‑H模型，将机器人连杆几何参数误差分别标记为Δθ(关节转角
误差)、Δα(关节扭角误差)、Δa(连杆长度误差)、Δd(关节距离误差)与Δβ(旋转角误差)。
i‑1 i‑1
在几何参数误差的影响下，齐次变换矩阵Ti 新增加微分齐次摄动矩阵dTi ：
[0072]
[0073] 考虑机器人每一个关节的几何参数误差，机器人末端位姿表示为：
[0074]
[0075] 考虑机器人基坐标系和测量坐标系之间的误差，可以得到机器人几何误差辨识模型：
[0076] ΔX＝A·ΔQ
[0077] 其中ΔX为末端定位误差，A为描述参数误差与定位误差相互关系的系数矩阵，ΔQ为参数误差。
[0078] S2：利用激光跟踪仪测量机器人末端执行器位置坐标，测量机器人实际位姿；
[0079] 利用控制器控制机器人在空间中运动，利用激光跟踪仪测量机器人末端执行器的位置坐标，同时读取并记录对应位置机器人关节参数，得到机器人大量的实际位姿数据。
[0080] S3：建立测量位姿综合层次评价指标，基于改进的序列浮动前向选择算法确定最优测量位姿；
[0081] 误差辨识模型系数矩阵A为m×n阶矩阵，系数矩阵奇异值σ1,σ2...σn，得到位姿观测性评价指标：
[0082]
[0083] 除了测量位姿的观测性指标之外，还需要考虑测量位姿的空间分布情况，包括位姿的离散性评价指标δ和均匀性评价指标U。记测量范围S中的关节角集合为C＝{θ1,θ2,...
θn}，θi对应末端位置为p(θi)，得到测量位姿的分布性指标为：
[0084]
[0085] 测量位姿的离散性评价指标为：
[0086]
[0087] 其中 表示测量点与中心点的平均距离，d(y,pk)表示y与pk之间的欧氏距离。
[0088] 利用反正切函数的非线性归一化，引入加权控制因子λ，控制评价指标中的每个组成部分的权值，得到位姿综合评价指标：
[0089]
[0090] 由于系数矩阵的可观测性指标对参数辨识结果重要性较大，选择O1指标作为一级指标，根据O1指标选择测量位姿得到集合S1，以位姿综合评价指标Oc作为二级指标，再对集
合S1进行优化。通过权值控制数据分布和观测性指标重要性，从集合S1中按照该指标选择得
到最优测量位姿S2，得到最优测量位姿问题的解集S2。
[0091] 进一步地，利用改进的浮动序列前向选择算法从最优测量位姿解集S2中确定测量位姿，算法具体步骤如下：
[0092] 1)初始化空的最优测量位姿解集ω，初始化算法参数，包括待选位姿集合 需求的位姿数m，迭代次数e，随机交换删除的位姿数量P以及初始解的位姿数量m等参数，从待选
位姿集合 中随机选择l个位姿作为初始解，开始算法迭代工作；
[0093] 2)序列前向添加，添加位姿时关注位姿观测性评价指标O1，从给定的待选位姿集合 中确定一个位姿，在添加该位姿后，使得最优测量位姿解集ω的观测性评价指标O1最
大，记“添加”步骤的位姿评价指标函数为fadd(θ)；
[0094] 3)浮动条件删除，删除位姿时仍关注位姿观测性评价指标O1，在最优解集ω中判断并删除不重要位姿，直到删除一个不重要位姿后观测性评价指标O1小于加入新位姿之
前，记“删除”步骤的位姿评价指标函数为frmv(θ)；
[0095] 4)随机交换与删除，为增加整个过程的随机性以避免解集陷入局部极值，采用综合考虑位姿观测性、分布性与离散性的评价指标Oc作为判断准则，通过权值控制弱化系数
矩阵的观测性，增加数据的分布性重要程度，在每一次迭代结束之前，依次从最优解集ω中
随机选择k个位姿，随机执行与待选位姿解集交换或者删除，根据综合评价指标Oc判断是否
执行交换或者删除，记此时的位姿评价指标函数为frand(θ)；
[0096] 5)循环迭代，判断迭代次数是否满足迭代要求，退出循环，得到实际测量的最优位姿Pt。
[0097] S4：利用机器人名义MD‑H值，计算得到不同关节角参数对应的理论位姿。
[0098] 利用S1中建立的机器人关节的相邻位姿变换矩阵，代入机器人名义MD‑H参数，得到机器人末端的名义位姿Pr
[0099] S5：实际测量位姿Pt与理论位姿Pr之差为机器人末端定位误差ΔX，根据已建立的机器人运动学误差模型，基于自适应临界值的抗差估计的方法进行辨识，消除测量过程中
测量扰动以及仪器本身误差的影响，提高辨识精度，得到最终的几何参数误差ΔQ；
[0100] 机器人末端定位误差ΔX可表示为：
[0101] ΔX＝Pt‑Pr
[0102] 在测量实际位姿时，由于测量仪器以及测量扰动等因素的影响，造成某些测量数据存在较大的波动，因此利用自适应临界值抗差估计的方法对存在粗差数据进行参数辨
识。
[0103] 自适应临界值抗差估计的方法的基本原理是通过修改最小二乘法的权值矩阵，在迭代拟合过程中不断减小误差较大的观测值对应的权值矩阵，抑制粗差。P是抗差估计最小
二乘法的权值矩阵，具体形式为：
[0104]
[0105] 在最小二乘迭代计算过程中，权值矩阵通过IGG3权值函数确定。根据每次迭代结果的残差分布状态计算指定置信水平的置信区间，权值函数的临界值根据置信区间的极限
值来自适应构造，消除手动设定临界值的风险。采用自适应临界值构造的权值函数为：
[0106]
[0107] 其中，k0、k1为调和系数， 为标准化后的残差，为下限值，为上限值。上式表明，对低于置信区间下限的观测值的权值进行保留，对位于区间内的观测值进行降权，大于
上限值的观测值直接舍弃。
[0108] 基于自适应构造临界值的抗差估计的方法，迭代进行几何误差辨识。迭代开始之初，设置权值矩阵P为单位矩阵，然后采用加权最小二乘进行几何参数误差的辨识，根据参
数误差辨识的结果计算残差分布状态，并基于自适应构造临界值的IGG3权值函数，调整权
值矩阵，再次迭代，直到达到辨识精度的阈值要求或者迭代次数，退出迭代，完成参数误差
辨识。
[0109] 利用本方法对为标定的机器人进行参数辨识，机器人的名义D‑H参数计算出对应85组关节角的末端位置和实测点位之间的差值得到参数误差辨识前的误差，如图2浅色圆
点曲线所示；利用本发明方法辨识后，辨识结果修正后的D‑H参数计算出机器人末端位姿和
实测点位之间的差值得到参数修正后的误差，如图2深色方点曲线所示。
[0110] 经对比，参数辨识前，机器人末端综合定位误差为1.5mm～3mm，经过本发明方法进行参数辨识后，定位误差减小到0.5mm～1.0mm，均值由辨识前的2.381mm降低到0.7728mm，
提升率接近70％，有效验证了本发明方法的有效性。
[0111] 以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例
所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修
改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的范围。
