机器人可变形操作臂参数标定方法

摘要

一种机器人可变形操作臂参数标定的方法。该方法针对机器人可变形操作臂在改变形状后需要重新标定自身参数的任务而提出。首先，给出了单目相机测量操作臂的位置和姿态的优化方法。然后，建立了可变形操作臂的D-H模型；最后，通过构造可变形操作臂参数的约束方程，求解并修正其参数。传统的操作臂参数标定方法，因成本高，操作复杂，不能适用于面向多种服务任务的可变形操作臂参数标定任务。本发明所述的标定方法，可以快速、准确地完成可变形操作臂在形状改变后的标定任务。仿真和实验表明，本发明简单实用，且对噪声具有很好的鲁棒性。

说明书

技术领域

本发明属于机器人操作臂标定的技术领域，特别是涉及一种可变形操作臂的标定方 法。

背景技术

众所周知，机器人操作臂自身参数标定是提高操作臂精度的一种有效方法。在得到精 确的操作臂自身参数后，操作臂的正运动学、逆运动学的准确度都会相应提高，从而提高 操作臂的作业精度。当前操作臂参数的标定方法，大多数应用于刚性的、物理结构不会发 生改变的操作臂。而对于新颖的可变形操作臂，由于其自身物理结构、形状会发生改变， 传统的应用于刚性操作臂标定方法已经不再适用。

可变形操作臂的连杆能够在三维立体空间中任意弯曲成一定形状，并能保持相应形 状，连杆弯曲时长度和大小基本稳定，且弯曲后具有一定的支撑力。该操作臂关节少，却 拥有多关节操作臂才拥有的丰富的工作空间，可执行多自由度操作臂才能完成的任务。并 且该操作臂具有多种用途，可实现复用同一支操作臂来适应多种多样的环境和任务。同时， 该操作臂具有被动柔性，可降低对外界人和物体的伤害，保障了服务对象的安全^[1]。可变 形操作臂拥有诸多益处，而操作臂自身标定问题又是提高其精度、保证其正常作业的重要 前提，因此，可变形操作臂的参数标定问题是亟待解决的。

以往的机器人操作臂参数标定的方法，主要分为两类：借助外部测量设备的参数标定 方法，及传统的利用相机的参数标定方法。这两类方法都有其自身的局限性，具体来说：

借助外部设备的标定方法，需要使用昂贵的、笨重的测量系统。其缺点是成本高，标 定时需要借助体积庞大的外部测量设备，使用不方便，因此这种方法不适合应用在家庭服 务机器人的家居环境中。

传统的单目相机参数标定方法，需要在机器人操作臂本体上装载单目相机，这种方法 多应用体积较大的工业操作臂中。其缺点是，装载单目相机使得操作臂变得更为臃肿，并 增加了操作臂的负载，为操作臂执行任务带来了不便。并且，家庭服务机器人的操作臂体 积小，可承受负载较低，无法装载一个单目相机。

发明内容

有鉴于此，本发明目的是克服现有技术存在的上述不足，提供一种机器人可变形操作 臂参数标定的方法。

本发明提供的机器人可变形操作臂参数标定的方法包括：

第1、给出单目相机测量操作臂的位置和姿态的方法；

第2、建立可变形操作臂的D-H模型；

第3、构造可变形操作臂形状改变后D-H参数的约束方程组；

第4、给出操作臂D‐H参数的求解方法，以及参数修正的方法。

其中，

第1步，给出单目相机测量操作臂的位置和姿态的方法

在操作臂关节处贴标志块的原则和方法，即利用单目相机测量标志块从而得到关节的 位置和姿态信息的方法。

针对图1所示可变形操作臂，其参数标定的过程具体方法是：

操作臂基座、关节3和关节4上分别贴有标志块0,标志块3，标志块4,标志块编号与 对应关节相吻合；不存在标志块1,标志块2。单目相机可以识别标志块的位置和姿态。

上述是基于4关节，2柔性连杆的可变形操作臂的参数标定方法。对于多关节、多柔 性连杆的可变形操作臂，在标定参数时，需要符合以下规则：

第1，发生形状改变的连杆两端关节上需要贴标志块。标志块位置尽量毗邻柔性连杆，会 为构建约束方程组提供方便。

第2，基座标系必须贴有标志块。

第3，标志块的最少数量m，取决于形状改变的连杆的数量n，m＝n-1。若标志块数目 大于n，会简化约束方程组的构造过程。

第2步所述建立可变形操作臂的D-H模型(Denavit-Hartenberg model)的方法如下：

图1指出本发明中可变性操作臂的D-H模型，以及标识块和相机的位置^[2]。表1是常 规D-H参数建模方法下的D-H参数表。

表1、常规D-H参数建模方法下的D-H参数表

i α_ia_iθ_id_i1 α₁a₁θ₁d₁2 α₂a₂θ₂d₂3 α₃a₃θ₃d₃4 α₄a₄θ₄d₄w 0 aw 0 0

${}_i{}^{i-1}T=\left(\begin{array}{cccc}\cos {\theta }_i& {-\sin \theta }_i& 0& a_{i-1}\\ \sin {\theta }_{i}c{\alpha }_{i-1}& \cos {\theta }_{i}c{\alpha }_{i-1}& -\sin {\alpha }_{i-1}& -\sin {\alpha }_{i-1}{d}_i\\ \sin {\theta }_{i}s{\alpha }_{i-1}& \cos {\theta }_{i}s{\alpha }_{i-1}& \cos {\alpha }_{i-1}& \cos {\alpha }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right)$

为坐标系{i}相对于坐标系{i-1}的齐次变换阵，w是操作臂末端坐标系。

${}_w{}^{0}T={}_1{}^{0}T{}_2{}^{1}T{}_3{}^{2}T{}_4{}^{3}T{}_w{}^{4}T$

为柔性操作臂末端坐标系与基坐标系和之间的齐次变换阵。

在可变形操作臂建立D-H模型的过程中，遇到的问题及解决方案是：

基坐标系与标志块0的坐标系相对位置已知且固定不变，基坐标系信息可由标志块0 的坐标系信息得到；即单目相机通过识别标志块0坐标信息，即可得基座标系与相机坐标 系间的齐次变换矩阵

关节4坐标系与标志块4的坐标系相对位置已知且固定不变，关节4坐标系信息可由 标志块4的坐标系信息得到；即单目相机通过识别标志块4的坐标系信息，即可得关节4 坐标系与相机坐标系间的齐次变换矩阵

关节3坐标系与标志块3的坐标系相对位置未知且会改变。单目相机通过识别标志块 3的坐标系信息，标志块3坐标系与相机坐标系间的齐次变换矩阵但未能得到坐 标系3与相机坐标系间的齐次变换矩阵关节3的坐标系{3}的原点已经不在操作臂 本体上，而是在关节的Z轴延长线上，它们之间有一个偏移距离d_33’。同时，我们发现， 关节3坐标系的X轴即X₃也发生了偏转，与标志块3坐标系的X轴即X_3’。不过，X₃与 X_3’都在一个平面内，它们都与Z₃垂直，X₃与X_3’之间有一个夹角θ_33'。

如此，建立可变形操作臂的D-H模型，其D-H参数如表2所示。

表2、D-H参数

i α_ia_iθ_id_i1 α₁a₁θ₁d₁2 α₂a₂θ₂d₂3′ α₃a₃θ₃+θ_33′d₃+d_33′4 α₄a₄θ₄d₄w 0 aw 0 0

表中α_i为关节i的Z轴与关节i-1的Z轴间的夹角，a_i为关节i的Z轴与关节i-1的Z 轴间的距离，θ_i为关节i的X轴与关节i-1的X轴间的夹角，d_i为关节i的X轴与关节i-1 的X轴间的距离，i＝1,2,3,4。θ_33'为关节3的X轴与关节3'的X轴间的夹角，d_33'为关节3 的X轴与关节3'的X轴间的距离。

3'为一个虚拟关节，即标志块3的坐标系信息，它是关节3和关节4间的一个桥梁。 在可变形操作臂的D-H参数表中，在柔性操作臂的形状发生改变后，D-H参数表中的部 分参数需要标定的。需要标定的12个参数是：α₃、α₄、a₃、a₄、d₂、d₃、d_33'、d₄、θ₂、 θ₃、θ_33'、θ₄。其余参数，是操作臂的已知量，且在操作臂形状发生改变的情况下，这些 参数亦不改变。

第3步所述的构造可变形操作臂形状改变后D-H参数的约束方程组的方法是：

为使得计算简便，可以令关节角θ₁＝0。

首先，构造参数标定的约束方程组1

单目相机可以得到和即可求

${}_4{}^{3^{\prime }}T={}_{\mathrm{camera}}{}^{3^{\prime }}T{}_{\mathrm{camera}}{}^{4}T=\left(\begin{array}{cccc}{q}_{11}& q_{12}& q_{13}& k_x\\ q_{21}& q_{22}& q_{23}& k_y\\ q_{31}& q_{32}& q_{33}& k_z\\ 0& 0& 0& 1\end{array}\right)$

其中q_ij(i,j＝1,2,3)和k_x、k_y、k_z为通过单目相机得到的已知量。

而在D-H模型下的表示为：

${}_4{}^{3^{\prime }}T={}_3{}^{3^{\prime }}T*{}_4{}^{3}T={}_3{}^{3^{\prime }}T^{-1}*{}_4{}^{3}T=\left(\begin{array}{cccc}\cos {\theta }_{{33}^{\prime }}& \sin {\theta }_{{33}^{\prime }}& 0& 0\\ -\sin {\theta }_{{33}^{\prime }}& \cos {\theta }_{{33}^{\prime }}& 0& 0\\ 0& 0& 1& -d_{{33}^{\prime }}\\ 0& 0& 0& 1\end{array}\right)$

由此可得参数标定的约束方程组1：

q₁₁＝cosθ_33'cosθ₄+cosα₄sinθ_33'sinθ₄

q₁₂＝-cosθ_33'sinθ₄+cosα₄sinθ_33'cosθ₄

q₁₃＝-sinα₄sinθ_33'

q₂₁＝-sinθ_33'cosθ₄+cosα₄cosθ_33'sinθ₄

q₂₂＝sinθ_33'sinθ₄+cosα₄cosθ_33'cosθ₄

q₂₃＝-sinα₄cosθ_33'

q₃₁＝sinθ₄sinα₄

q₃₂＝cosθ₄sinα₄

q₃₃＝cosα₄

q_x＝a₄cosθ_33'-d₄sinα₄sinθ_33'

q_y＝-a₄sinθ_33'-d₄sinα₄cosθ_33'

q_z＝-d_33'+d₄cosα₄。

再次，构造参数标定的约束方程组2

单目相机可得和

则${}_{3^{\prime }}{}^{0}T={}_{\mathrm{camera}}{}^{0}T{\left({}_{\mathrm{camera}}{}^{3^{\prime }}T\right)}^{-1}=\left(\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right)$

其中r_ij(i,j＝1,2,3)和p_x、p_y、p_z为通过单目相机得到的已知量。

而在D-H模型下的表示为：

${}_{3^{\prime }}{}^{0}T={}_1{}^{0}T_2^{1}T{}_3{}^{2}T{}_{3^{\prime }}{}^{3}T={}_3{}^{0}T\left(\begin{array}{cccc}\cos {\theta }_{{33}^{\prime }}& -\sin {\theta }_{{33}^{\prime }}& 0& 0\\ \sin {\theta }_{{33}^{\prime }}& \cos {\theta }_{{33}^{\prime }}& 0& 0\\ 0& 0& 1& d_{{33}^{\prime }}\\ 0& 0& 0& 1\end{array}\right)$

由此可得参数标定的约束方程组2：

r₁₁＝cos(θ₃+θ_33')cosθ₂-sin(θ₃+θ_33')cosα₃sinθ₂

r₁₂＝-sin(θ₃+θ_33')cosθ₂-cos(θ₃+θ_33')cosα₃sinθ₂

r₁₃＝sinα₃sinθ₂

r₂₁＝sin(θ₃+θ_33')sinα₃

r₂₂＝cos(θ₃+θ_33')sinα₃

r₂₃＝cosα₃

r₃₁＝-cos(θ₃+θ_33')sinθ₂-sin(θ₃+θ_33′)cosα₃cosθ₂

r₃₂＝-sin(θ₃+θ_33')sinθ₂+cos(θ₃+θ_33')cosα₃cosθ₂

r₃₃＝sinα₃cosθ₂

p_x＝a₃cosθ₂+(d₃+d_33')sinα₃sinθ₂

p_y＝d₂+(d₃+d_33')cosα₃

p_z＝-a₃sinθ₂+(d₃+d_33')sinα₃cosθ₂+d₁。

其中，α_i为关节i的Z轴与关节i-1的Z轴间的夹角，a_i为关节i的Z轴与关节i-1 的Z轴间的距离，θ_i为关节i的X轴与关节i-1的X轴间的夹角，d_i为关节i的X轴与关 节i-1的X轴间的距离，i＝1,2,3,4。3'为一个虚拟关节，即标志块3的坐标系信息。θ_33'为 关节3的X轴与关节3'的X轴间的夹角，d_33'为关节3的X轴与关节3'的X轴间的距离。 r_ij(i,j＝1,2,3)，q_ij(i,j＝1,2,3)和p_x、p_y、p_z、k_x、k_y、k_z为通过单目相机得到的已知 量。

第4步所述操作臂D‐H参数的求解方法，即求解约束方程组的方法是：

首先，求解约束方程组1

a.当sinα₄≠0时，

${\alpha }_4=a\tan 2(\pm \sqrt{1-q_{33}^2},q_{33})$

θ₄＝atan2(q₃₁/sinα₄,q₃₂/sinα₄)

θ_33'＝atan2(-q₁₃/sinα₄,-q₂₃/sinα₄)

d₄＝(q_xsinθ_33'+q_ycosθ_33')/(q₁₃sinθ_33'+q₂₃cosθ_33')

d_33'＝d₄q_33'-q_z

a₄＝(q_xq₂₃+q_yq₁₃)/(sinθ_33'q₁₃+cosθ_33'q₂₃)

d₄、d_33'不受sinα₄符号影响。由于sinα₄符号不确定，而α₄、θ₄、θ_33'、a₄又与sinα₄符号有关，故有2组解。

可利用a₄>0来判断sinα₄符号，也就使得α₄、θ₄、θ_33'、d₄、d_33'、a₄有唯一解。

b.当sinα₄＝0时，只能求出θ_33'±θ₄，±d₄-d_33'。

θ_33'±θ₄＝atan2(-q₂₁,q₁₁)；cosα₃＝-1时取“+”，cosα₃＝1时取“-”。

±d₄-d_33'＝q_z；cosα₃＝1时取“+”，cosα₃＝-1时取“-”。

再次，求解约束方程组2

a.当sinα₃≠0，sinα₄≠0时，

${\alpha }_3=a\tan 2(\pm \sqrt{1-r_{23}^2},r_{23})$

θ₂＝atan2(r₁₃/sinα₃,r₃₃/sinα₃)

θ₃＝atan2(r₂₁/sinα₃,r₂₂/sinα₃)-θ_33'

d₃＝[sinθ₂p_x+(p_z-d₁)cosθ₂]/(r₁₃sinθ₂+r₃₃cosθ₂)-d₃₃

d₂＝(d_33'+d₃)r₂₃-p_y

a₃＝[p_xr₃₃+(p_z-d₁)r₁₃]/(r₃₃cosθ₂+r₁₃sinθ₂)

d₂、d₃不受sinα₃符号影响。但由于sinα₃符号不确定，而θ₂、θ₃、θ_33'、a₃又与sinα₃符号有关，故有2组解。

可利用a₃>0来判断sinα₃符号，也就使得d₂、d₃、a₃、θ₂、θ₃、α₃有唯一解。

b.当sinα₃≠0，sinα₄＝0时，

${\alpha }_3=a\tan 2(\pm \sqrt{1-r_{23}^2},r_{23})$

θ₂＝atan2(r₁₃/sinα₃,r₃₃/sinα₃)

d₂＝(d_33'+d₃)r₂₃-p_y

a₃＝[p_xr₃₃+(p_z-d₁)r₁₃]/(r₃₃cosθ₂+r₁₃sinθ₂)

θ₃+θ_33'＝atan2(r₂₁/sinα₃,r₂₂/sinα₃)

d₃+d₃₃＝[p_xsinθ₂+(p_z-d₁)cosθ₂]/(r₁₃sinθ₂+r₃₃cosθ₂)

c.当sinα₃＝0，sinα₄≠0时，

θ₂＝atan2(d₁-p_z,p_x)

a₃＝(p_x+p_z-d₁)/(cosθ₂-sinθ₂)

θ₃＝±[atan2(r₃₁,r₁₁)-θ₂]-θ_33'；cosα₃＝1时取“+”，cosα₃＝-1时取“-”。

d₂±(d₃+d₃₃)＝p_y；cosα₃＝1时取“+”，cosα₃＝-1时取“-”。

d.当sinα₃＝0，sinα₄＝0时，

θ₂＝atan2(d₁-p_z,p_x)

a₃＝(p_x+p_z-d₁)/(cosθ₂-sinθ₂)

θ₃+θ_33'＝±[atan2(r₃₁,r₁₁)-θ₂]；cosα₃＝1时取“+”，cosα₃＝-1时取“-”。

d₂±(d₃+d₃₃)＝p_y；cosα₃＝1时取“+”，cosα₃＝-1时取“-”。

最后，参数修正方法：

利用单目相机，多次测量和从而求得多组利用扩展卡尔曼滤波法 不断修正各参数的值，具体方法详见参考文献^[3]。

本发明的优点和有益效果

本发明利用一个单目相机，以及操作臂上贴有的若干标志块，就可以得到操作臂的位 置和姿态信息，从而标定出操作臂的参数。这种方法实施简单、成本较低，适用面广。

附图说明：

图1为可变形操作臂。

图2为参数标定过程中的D-H模型。

图中，1是第一关节，2是第二关节，3是第三关节，4是第四关节，5是标志块0，6 是标志块3，7是标志块4，8是操作臂基座，9是操作臂可变形的柔性连杆，10是单目相 机。

具体实施方式

第1步、给出单目相机测量操作臂的位置和姿态的方法；

此可变形操作臂包括4个关节，2个可变形连杆。操作臂基座、关节3和关节4上分 别贴有标志块0,标志块3，标志块4,标志块编号与对应关节相吻合；不存在标志块1,标志 块2。单目相机可以识别标志块的位置和姿态。

第2步，可变形操作臂的D-H模型(Denavit-Hartenberg model)

如图1所示，建立D-H参数模型，得到DH参数表：见表3。

表3、DH参数表

i α_ia_iθ_id_i1 0 0 θ₁35 2 -90° 0 θ₂0 3′ 0 a₃θ₃+θ_33′d₃+d_33′4 α₄a₄θ₄d₄w 0 95 0 0

第3步，构造可变形臂约束方程组

首先，构造参数标定的约束方程组1

单目相机可以得到和即可求

${}_4{}^{3^{\prime }}T={}_{\mathrm{camera}}{}^{3^{\prime }}T{}_{\mathrm{camera}}{}^{4}T=\left(\begin{array}{cccc}{q}_{11}& q_{12}& q_{13}& k_x\\ q_{21}& q_{22}& q_{23}& k_y\\ q_{31}& q_{32}& q_{33}& k_z\\ 0& 0& 0& 1\end{array}\right)$

其中q_ij(i,j＝1,2,3)和k_x、k_y、k_z为已知量。

而在D-H模型下的表示为：

${}_4{}^{3^{\prime }}T={}_3{}^{3^{\prime }}T{}_4{}^{3}T={\left({}_{3^{\prime }}{}^{3}T\right)}^{-1}{}_4{}^{3}T=\left(\begin{array}{cccc}\cos {\theta }_{{33}^{\prime }}& \sin {\theta }_{{33}^{\prime }}& 0& 0\\ -\sin {\theta }_{{33}^{\prime }}& \cos {\theta }_{{33}^{\prime }}& 0& 0\\ 0& 0& 1& -d_{{33}^{\prime }}\\ 0& 0& 0& 1\end{array}\right)$

由此可得参数标定的约束方程组1：

q₁₁＝cosθ_33'cosθ₄+cosα₄sinθ_33'sinθ₄

q₁₂＝-cosθ_33'sinθ₄+cosα₄sinθ_33'cosθ₄

q₁₃＝-sinα₄sinθ_33'

q₂₁＝-sinθ_33'cosθ₄+cosα₄cosθ_33'sinθ₄

q₂₂＝sinθ_33'sinθ₄+cosα₄cosθ_33'cosθ₄

q₂₃＝-sinα₄cosθ_33'

q₃₁＝sinθ₄sinα₄

q₃₂＝cosθ₄sinα₄

q₃₃＝cosα₄

q_x＝a₄cosθ_33'-d₄sinα₄sinθ_33'

q_y＝-a₄sinθ_33'-d₄sinα₄cosθ_33'

q_z＝-d_33'+d₄cosα₄。

再次，构造参数标定的约束方程组2

单目相机可得和

则${}_{3^{\prime }}{}^{0}T={}_{\mathrm{camera}}{}^{0}T{\left({}_{\mathrm{camera}}{}^{3^{\prime }}T\right)}^{-1}=\left(\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right)$

其中r_ij(i,j＝1,2,3)和p_x、p_y、p_z为已知量。

而在D-H模型下的表示为：

${}_{3^{\prime }}{}^{0}T={}_1{}^{0}T_2^{1}T{}_3{}^{2}T{}_{3^{\prime }}{}^{3}T={}_3{}^{0}T\left(\begin{array}{cccc}\cos {\theta }_{{33}^{\prime }}& -\sin {\theta }_{{33}^{\prime }}& 0& 0\\ \sin {\theta }_{{33}^{\prime }}& \cos {\theta }_{{33}^{\prime }}& 0& 0\\ 0& 0& 1& d_{{33}^{\prime }}\\ 0& 0& 0& 1\end{array}\right)$

由此可得参数标定的约束方程组2：

r₁₁＝cos(θ₃+θ_33')cosθ₂-sin(θ₃+θ_33')cosα₃sinθ₂

r₁₂＝-sin(θ₃+θ_33')cosθ₂-cos(θ₃+θ_33')cosα₃sinθ₂

r₁₃＝sinα₃sinθ₂

r₂₁＝sin(θ₃+θ_33')sinα₃

r₂₂＝cos(θ₃+θ_33')sinα₃

r₂₃＝cosα₃

r₃₁＝-cos(θ₃+θ_33')sinθ₂-sin(θ₃+θ_33')cosα₃cosθ₂

r₃₂＝-sin(θ₃+θ_33')sinθ₂+cos(θ₃+θ_33')cosα₃cosθ₂

r₃₃＝sinα₃cosθ₂

p_x＝a₃cosθ₂+(d₃+d_33')sinα₃sinθ₂

p_y＝d₂+(d₃+d_33')cosα₃

p_z＝-a₃sinθ₂+(d₃+d_33')sinα₃cosθ₂+d₁。

第4步，求解约束方程组

求得的各参数填入D-H表，见表4。

表4、

i α_ia_iθ_id_i1 0 0 0 35 2 -90° 0 5.728° -150.60 3 -81.97° 269.46 17.11° -116.54 4 138.73° 160.08 5.729° 29.13 w 0 95 0 0

利用扩展卡尔曼滤波方法参数修正后的D-H表见表5：

表5、

i α_ia_iθ_id_i1 0 0 0 35 2 -90° 0 5.91° -150.24 3 -82.06° 270.54 17.26° -117.33 4 138.44° 159.68 5.83° 29.84 w 0 95 0 0

。

参考文献：

[1]刘景泰，于宁波，许林，张森，郝洁，用于服务机器人的柔性操作臂[P]，201310173918.1

[2]John J.Craig,Introduction to Robotics Mechanics and Control[M],2006.6

[3]Greg Welch,Gary Bishop,An Introduction to the Kaman Filter,SIGGRAPH2001，Los  Angeles,CA,August12-17,2001。