CN102314690A - 机械臂运动学参数分离辨识方法 - Google Patents

Abstract

基于激光跟踪仪位置测量原理和机械臂微分运动原理，通过将末端执行器齐次变换矩阵中的姿态矩阵和位置向量进行分离，提出一种无需计入关节位置误差的机械臂D-H参数辨识方法。首先，通过测得末端执行器在基系下的姿态矩阵对姿态参数进行辨识；然后，利用辨识好的姿态参数和实测的末端位置对分离后的位置参数进行辨识；最终实现机械臂运动学参数辨识。

Description

机械臂运动学参数分离辨识方法

技术领域

本发明涉及机器人标定领域，尤其涉及一种将姿态参数和位置参数分离辨识的机械臂运动学参数辨识方法。

背景技术

机器人参数辨识是提高精度的重要手段，对于机器人离线编程和视觉引导下操作的成功具有重要的意义。机器人各连杆的几何参数误差是造成机器人定位误差的最主要环节，它主要是由于制造和安装过程中产生的连杆实际几何参数与名义参数值之间的偏差造成的，一般被视为系统误差。此外，一些影响因素如环境、机械变形被视为随机误差。所谓辨识就是运用先进的测量手段和适当的参数识别方法辨识出机器人模型的准确参数，从而提高机器人精度的过程。

目前，对机器人运动学参数的辨识，国内外进行了大量的研究。通常采用的方法是先建立适当的运动学误差模型，然后精确测量几组位姿，最后获得实际模型参数并运用正向运动学求解实际位姿。

发明内容

本发明的目的是针对目前运动学标定方法的不足提供一种参数分离辨识方法，该方法操作简单且精度大大提高。

其技术解决方案包括如下步骤：

(1)利用激光跟踪仪建立基坐标系∑_B和末端坐标系∑_T。将三靶球与末端执行器固连，采用三点不共线布点结构建立辅助坐标系∑_F。利用坐标系之间的转化关系，求得末端坐标系∑_T在基坐标系∑_B下的姿态矩阵R。根据姿态矩阵R和所建立的姿态参数误差模型，对姿态参数进行标定。

(2)利用实测的末端执行器位置和辨识好的姿态参数对分离后的位置参数进行辨识，最终实现机械臂运动学参数辨识。

本发明的优点是：

(1)通过将末端执行器齐次变换矩阵中的姿态矩阵和位置向量进行分离，有效防止了数量级较小的姿态误差被数量级较大的位置误差淹没。

(2)提出一种无需计入关节位置误差的机械臂D-H参数辨识方法，避免关节位置测量误差的同时，保证了齐次变换矩阵误差补偿的高精度特征。

(3)利用激光跟踪仪，采用三点不共线布点结构建立靶座辅助坐标系求得末端执行器在基系下的姿态矩阵，克服了传统等腰三角形布点结构的局限性。

附图说明

图1是机器人运动学参数辨识流程图。

图2是姿态测量中各坐标系之间关系。

具体实施方式

(1)修正D-H模型

经典的D-H模型中相邻两轴平行或接近平行时存在奇异点，在此基础上增加一项绕y轴旋转β角度的变换，以克服奇异点问题，修正好的坐标变换矩阵为：

$T_i^{-1}=\mathrm{Rot}(x,{\mathrm{\α}}_{i-1})\mathrm{Trans}(x,a_{i-1})\mathrm{Rot}(z,{\mathrm{\θ}}_i)\mathrm{Trans}(z,d_i)\mathrm{Rot}(y,{\mathrm{\β}}_i)$

$=\left[\begin{array}{cccc}{\mathrm{c\θ}}_i{\mathrm{c\β}}_i& -s{\mathrm{\θ}}_i& {\mathrm{c\θ}}_i\mathrm{s\β}& a_{i-1}\\ {\mathrm{s\θ}}_i{\mathrm{c\α}}_{i-1}{\mathrm{c\β}}_i+{\mathrm{s\α}}_{i-1}{\mathrm{s\β}}_i& {\mathrm{c\θ}}_i{\mathrm{c\α}}_{i-1}& {\mathrm{s\θ}}_i{\mathrm{c\α}}_{i-1}{\mathrm{s\β}}_i-{\mathrm{s\α}}_{i-1}{\mathrm{c\β}}_i& -d_i{\mathrm{s\α}}_{i-1}\\ {\mathrm{s\θ}}_i{\mathrm{s\α}}_{i-1}{\mathrm{c\β}}_i-{\mathrm{c\α}}_{i-1}{\mathrm{s\β}}_i& {\mathrm{c\θ}}_i{\mathrm{s\α}}_{i-1}& {\mathrm{s\θ}}_i{\mathrm{s\α}}_{i-1}{\mathrm{s\β}}_i+{\mathrm{c\α}}_{i-1}{\mathrm{c\β}}_i& d_i{\mathrm{c\α}}_{i-1}\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

对于公式(1)，当相邻两轴不平行时β＝0、d≠0，当相邻两轴平行或近似平行时d＝0、β≠0。根据正向运动学求解可得到机器人末端执行器到基坐标系的齐次坐标变换矩阵

$T_n^0=T_1^{0}T_2^1\·\·\·T_i^{i-1}\·\·\·T_n^{n-1}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_{3\×3}& P_{3\×1}\\ 0& 1\end{array}\right]---\left(2\right)$

(2)建立误差模型

齐次坐标变换矩阵

主要有旋转矩阵R和平移向量P组成。其中，旋转矩阵R是{α，θ，β}的函数，平移向量P是{α，θ，β，a，d}的函数。关节旋转角度θ所带来的误差对于机械臂末端位姿精度影响巨大，但该类误差通常受伺服特性和外部扰动的影响而难以确定。因此，在建立误差模型时将θ排除在外，通过补偿{α，β，a，d}来弥补θ所带来的关节误差。

本发明主体思路为：首先利用旋转矩阵R建立的姿态误差模型对{α，β}两组参数进行标定，再利用辨识好的{α，β}和位置误差模型对{a，d}进行标定，从而分两步将机器人D-H参数误差进行辨识。

建立姿态误差模型

设σ_z、ζ_y、γ_x分别代表末端参考系绕基座坐标系相应轴的转角，根据旋转矩阵与欧拉角之间的转化公式可得：

$\left\{\begin{array}{c}{\mathrm{\ζ}}_y=A\tan 2(-n_z,\sqrt{{n_x}^2+{n_y}^2})\\ {\mathrm{\σ}}_z=A\tan 2(n_y,n_x)\\ {\mathrm{\γ}}_x=A\tan 2(o_z,a_z)\end{array}\right.---\left(3\right)$

根据理想D-H参数，由公式(2)可求得机器人末端执行器在基坐标系下的名义旋转矩阵R。由公式(3)，可将旋转矩阵R转化为欧拉角Q＝(σ_z，ζ_y，γ_x)^T＝G(α，β)。假设机器人末端执行器在基坐标系下用欧拉角表示的真实姿态为：Q′＝(σ_z′，ζ_y′，γ_x′)^T＝G(α′，β′)＝G(α+Δα，β+Δβ)，Q′所对应的旋转矩阵为R′。其中：Δα、Δβ为α、β的偏差。

首先建立误差模型ΔQ＝Q′-Q。由于几何参数误差足够小，故可以利用微分运动学模型来近似代替误差方程，即对上式运动学方程进行全微分，可得：

$\mathrm{\ΔQ}=\underset{i=0}{\overset{n-1}{\mathrm{\Σ}}}\frac{\∂Q}{{\∂\mathrm{\α}}_i}{\mathrm{\Δ\α}}_i+\frac{\∂Q}{{\∂\mathrm{\β}}_k}{\mathrm{\Δ\β}}_k---\left(4\right)$

其中，ΔQ＝(Δσ_z，Δζ_y，Δγ_x)^T＝(σ_z′-σ_z，ζ_y′-ζ_y，γ_x′-γ_x)^T，β_k代表第k-1和k关节轴线平行或近似平行的情况。

令：ΔX＝(Δα₀…Δα_n-1，Δβ_k)^T；

$J=\left[\begin{array}{cccc}\frac{{\∂\mathrm{\σ}}_z}{\∂{\mathrm{\α}}_0}& \·\·\·& \frac{{\∂\mathrm{\σ}}_z}{{\∂\mathrm{\α}}_{n-1}}& \frac{\∂\mathrm{\σ}}{{\∂\mathrm{\β}}_k}\\ \frac{{\∂\mathrm{\ζ}}_y}{{\∂\mathrm{\α}}_0}& \·\·\·& \frac{{\∂\mathrm{\ζ}}_y}{{\∂\mathrm{\α}}_{n-1}}& \frac{{\∂\mathrm{\ζ}}_y}{{\∂\mathrm{\β}}_k}\\ \frac{{\∂\mathrm{\γ}}_x}{{\∂\mathrm{\α}}_0}& \·\·\·& \frac{{\∂\mathrm{\γ}}_x}{{\∂\mathrm{\α}}_{n-1}}& \frac{{\∂\mathrm{\γ}}_x}{{\∂\mathrm{\β}}_k}\end{array}\right]$

则公式(4)可表示为：ΔQ＝J·ΔX。因此可得：

ΔX＝J⁺·ΔQ    (5)

其中，J⁺为雅克比矩阵J的伪逆。将所求的连杆参数误差ΔX加到机器人给出的连杆参数α、β上，就可以得到新的一组α、β，利用牛顿迭代法反复进行迭代直到ΔG满足精度要求。

建立位置误差模型

由公式(2)可求得机器人末端执行器在基坐标系下的名义位置，即P＝(p_x，p_y，p_z)^T＝F(a，d)，将机器人末端执行器在基坐标系中的实际位置记为P′＝(p_x′，p_y′，p_z′)^T＝F(a′，d′)＝F(a+Δa，d+Δd)。

由于α、β在上步中已经辨识，所以在此只需对a、d进行辨识。按照姿态误差模型建立的原理可以将ΔP表示为：

$\mathrm{\ΔP}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂P}{{\∂a}_i}{\mathrm{\Δa}}_i+\underset{i=1}{\overset{k-1}{\mathrm{\Σ}}}\frac{\∂P}{{\∂d}_i}{\mathrm{\Δd}}_i+\underset{i=k+1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂P}{{\∂d}_i}{\mathrm{\Δd}}_i---\left(6\right)$

其中：ΔP＝(ΔP_x，ΔP_y，ΔP_z)^T＝(p_x′-p_x，p_y′-p_y，p_z′-p_z)^T。

令：ΔY＝(Δa₀…Δa_n-1，Δd₁…Δd_k-1，Δd_k+1…Δd_n)^T；

$L=\left[\begin{array}{ccccccccc}\frac{{\∂P}_x}{{\∂a}_1}& \·\·\·& \frac{{\∂P}_x}{{\∂a}_n}& \frac{{\∂P}_x}{{\∂d}_1}& \·\·\·& \frac{{\∂P}_x}{{\∂d}_{k-1}}& \frac{{\∂P}_x}{{\∂d}_{k+1}}& \·\·\·& \frac{{\∂P}_x}{{\∂d}_n}\\ \frac{{\∂P}_y}{{\∂a}_1}& \·\·\·& \frac{{\∂P}_y}{{\∂a}_n}& \frac{{\∂P}_y}{{\∂P}_1}& \·\·\·& \frac{{\∂P}_y}{{\∂d}_{k-1}}& \frac{{\∂P}_y}{{\∂d}_{k+1}}& \·\·\·& \frac{\∂P_y}{{\∂d}_n}\\ \frac{{\∂P}_z}{{\∂a}_1}& \·\·\·& \frac{{\∂P}_y}{{\∂a}_n}& \frac{{\∂P}_z}{{\∂d}_1}& \·\·\·& \frac{{\∂P}_z}{{\∂d}_{k-1}}& \frac{{\∂P}_z}{{\∂d}_{k+1}}& \·\·\·& \frac{{\∂P}_z}{{\∂d}_n}\end{array}\right]$

同理，可得：

ΔY＝L⁺·ΔP        (7)

其中，L⁺为雅克比矩阵L的伪逆。将所求的连杆参数误差ΔY加到机器人给出的连杆参数a、d上，就可以得到新的一组a、d，利用牛顿迭代法反复进行迭代直到ΔP满足精度要求。

(3)数据测量

测量数据包括两部分：姿态矩阵和位置向量。位置向量的确定采用激光跟踪仪点测量方法；而末端姿态矩阵的测量比较复杂，具体操作流程为：

1)在激光跟踪仪下分别建立基坐标系∑_B和与末端执行器固连的末端坐标系∑_T，则这两个坐标满足如下关系：

$P_B=P_{\mathrm{TO}}^B+R_T^T{}_{B}P---\left(8\right)$

其中，^BP、^TP为靶球在∑_B和∑_T下的坐标，

为∑_T到∑_B下的姿态变换矩阵，^BP_TO为∑_T原点在∑_B下的坐标。

2)在机器人末端固定三个靶座1、2、3(不能成直线)，并记录三个靶球在∑_B和∑_T下的坐标，分别记为^BP₁、^BP₂、^BP₃和^TP₁、^TP₂、^TP₃。由于∑_T与末端执行器是固连的，所以无论机器人各关节如何旋转^TP₁、^TP₂、^TP₃是不变的。

3)如图2所示构造辅助坐标系∑_f，其构造原则：P₁点为坐标系原点，向量

为x轴方向，z轴方向为向量

和

叉乘所确定的方向，y轴由右手定则确定。

4)设∑_f在∑_B中的姿态矩阵为

∑_f在∑_T中的姿态矩阵为具体求解如下：

令x_f，y_f，z_f轴的单位矢量分别为i_f，j_f，k_f，它们在末端坐标系中的方向余弦分别用(u_ix，u_iy，u_iz)^T，(u_jx，u_jy，u_jz)^T，(u_kx，u_ky，u_kz)^T表示。按照第三步中辅助坐标系建立规则，i_f、j_f、k_f可分别表示为：

$i_f=\frac{P_2^T-P_1^T}{|P_2^T-P_1^T|}=u_{\mathrm{ix}}{i}_T+u_{\mathrm{iy}}{j}_T+u_{\mathrm{iz}}{k}_T---\left(9\right)$

$k_f=\frac{(P_2^T-P_1^T)\×(P_3^T-P_1^T)}{|(P_2^T-P_1^T)\×(P_3^T-P_1^T)|}=u_{\mathrm{kx}}{i}_T+u_{\mathrm{ky}}{j}_T+u_{\mathrm{kz}}{k}_T---\left(10\right)$

$j_f=k_f\×i_f=\left|\begin{array}{ccc}{i}_T& j_T& k_T\\ u_{31}& u_{32}& u_{33}\\ u_{11}& u_{12}& u_{13}\end{array}\right|=u_{\mathrm{jx}}{i}_T+u_{\mathrm{jy}}{j}_T+u_{\mathrm{jz}}{k}_T---\left(11\right)$

其中i_T，j_T，k_T为末端坐标系三坐标轴的单位矢量。由姿态变换矩阵的定义可得辅助坐标系到末端坐标系的姿态变换矩阵为：

$R_f^T=\left[\begin{array}{ccc}{u}_{\mathrm{ix}}& u_{\mathrm{jx}}& u_{\mathrm{kx}}\\ u_{\mathrm{iy}}& u_{\mathrm{jy}}& u_{\mathrm{ky}}\\ u_{\mathrm{iz}}& u_{\mathrm{jz}}& u_{\mathrm{kz}}\end{array}\right]---\left(12\right)$

同理可求得∑_f在∑_B中的姿态矩阵为

则∑_T在∑_B下的姿态表示为：

$R_T^B=R_f^B\·{\left(R_f^T\right)}^{-1}---\left(13\right)$

机器人末端坐标系在基坐标系下用欧拉角表示的姿态可由旋转矩阵经公式(3)转化而得。

(4)参数辨识

机械臂运动学参数辨识的流程如图1所示，具体步骤为：

1)利用公式(13)所得的

和所建立的姿态误差模型对{α，β}两组参数进行标定。

2)利用1)中辨识好的{α，β}和位置误差模型对{a，d}进行标定。

3)验证标定后机器人末端位姿和实测位姿之间的偏差是否在要求范围内，假如不在精度范围内，对标定参数进行迭代，直到精度满足要求为止。

Claims (3)

1.通过将末端执行器齐次变换矩阵中的姿态矩阵和位置向量进行分离，分别建立姿态误差模型和位置误差模型，将运动学参数分离辨识，有效防止了数量级较小的姿态误差被数量级较大的位置误差淹没。

2.提出一种无需计入关节位置误差的机械臂D-H参数辨识方法，在建立误差模型时将θ排除在外，通过补偿{α，β，a，d}来弥补θ所带来的关节误差，避免关节位置测量误差的同时，保证了齐次变换矩阵误差补偿的高精度特征。

3.利用激光跟踪仪建立基坐标系和与末端执行器固连的末端坐标系，并采用三点不共线布点结构建立靶座辅助坐标系，利用三个坐标系之间的旋转关系求得末端执行器在基系下的姿态矩阵，克服了传统等腰三角形布点结构的局限性。

Images