一种蛇形机械臂视觉反馈与自主回零的方法与流程

1.本发明属于蛇形机械臂的技术领域；具体涉及一种蛇形机械臂视觉反馈与自主回零的方法。


背景技术：

2.蛇形机械臂是仿生学机器人的典型代表，具有柔性以及灵活度高的特点，相比于传统刚性结构机械臂有更强的适应能力，目前广泛应用于医疗手术、航空发动机探伤检测等诸多领域。蛇形机械臂结构可以具备小型化、轻型化等难以替代的优势，因此在未来智能仿生机器人领域中有很大潜力。
3.蛇形机械臂是一种具有广泛应用前景的连续型超冗余机械臂，在面对复杂、恶劣环境工作时，要求机械臂具备对环境感知能力。
4.传统视觉标定常用张正友的平面棋盘格标定方法，棋盘是一块由黑白方块组成的网状标定板，棋盘格作为相机标定的标定物，此种标定方法是一种被大众广泛应用的相机参数标定方法。该种标定方法简单、易操作，但是不包含编码信息，不利于对蛇形机器臂关节编号。


技术实现要素：

5.本发明解决传统蛇形机械臂任意位置回零困难的问题，进而提出基于aruco标识的蛇形机械臂关节角辨识方法，能够测量机械臂关节角，从而用于蛇形臂的自主回零过程。
6.本发明通过以下技术方案实现：
7.一种蛇形机械臂视觉反馈与自主回零的方法，所述方法包括以下步骤：
8.步骤1：通过aruco辨识方法测量关节角，利用相机外参数与关节角之间存在的关系，利用aruco关节角辨识方法测量蛇形机械臂每一个关节的实时关节角；所述相机外参数为矩阵t，t＝[r,t]，其中r为3
×
3的正交单位矩阵t为三维平移向量，矢量获得各个aruco标识码坐标系相对于相机坐标系的变换关系，在通过解算机械臂关节的id号，就能确定aruco标识码与关节号之间对应关系；
[0009]
步骤2：通过aruco辨识方法测量关节角后，测得的实时关节用于蛇形机械臂自主回零，由全局单目相机测得实时各个关节的关节角度，与角度期望值做差后作为角度误差输入进pd控制器，得到关节角速度，经雅克比矩阵得到绳索速度在通过积分器得到绳索变化量，进而构成自主回零闭环控制。
[0010]
进一步的，先求各个aruco标识码坐标系(o1,o2,o3,o4,o5)相对于基座坐标系的姿态变换矩阵
[0011][0012]
其中，t4为相机坐标系至基座坐标系的姿态变换矩阵，
[0013]
t4＝t2t
1t
t3ꢀꢀ
(2)
[0014]
r
ic
为第i个aruco标识码相对于相机坐标系的旋转矩阵，是单目相机量测值。
[0015]
进一步的，所述步骤1中机械臂关节id号的计算，根据aruco标识码坐标系的轴与机械臂关节的z轴向平行解算关节角，轴在上一个关节的关节坐标系坐标系下的坐标分量n
iy
为，
[0016][0017]
其中，为第i关节aruco标识码坐标系至第i-1关节坐标系的姿态变换矩阵如下，
[0018][0019]
其中，r
i-1,0
为关节坐标系相对基座坐标系的旋转矩阵，绕x,y轴旋转的关节角α
i
，β
i
可以由下列方程求出，
[0020]
n
iy
＝rot
y
(β
i
)rot
x
(α
i
)[0 0 1]
t
ꢀꢀ
(5)
[0021]
其中rot
x
(α)为绕x轴旋转α角度的转转矩阵，rot
y
(b)为绕y轴旋转β角度的旋转矩阵，r
i-1,0
可以由如下公式求得，
[0022]
r
i，0
＝r
i，i-1
r
i-1，i-2
…
r
1，0
r
0，0
ꢀꢀ
(6)
[0023]
其中，均表示第i关节aruco标识码坐标系至第i-1关节坐标系的姿态变换矩阵，当计算第一关节相对于基座坐标系的变换矩阵时可以由算得，从i≥2时由计算，而后带入(4)求得
[0024]
最后由式(3)和式(5)联立即可求得，绕x轴旋转的关节角α和绕y轴旋转的关节角β，α和β即为基于aruco辨识方法测量的关节角。
[0025]
进一步的，所述步骤2具体为，首先求得绳索长度与关节速度的雅克比矩阵设为j
t
，可通过绳索拉力与等效力矩之间的映射关系求得，
[0026][0027][0028]
其中，z0,z1,
…
z
2i-1
为d-h坐标系的z轴在惯性空间中的单位向量，p0,p1,...p
2i
为d-h坐标系的原点在惯性空间中的矢量坐标。
[0029]
进一步的，所述公式(8)变换成，
[0030]
[0031]
其中，驱动力在椎段i的体坐标系原点处产生的力旋量为
[0032][0033]
干扰力在椎段i的体坐标系原点处产生的力旋量为
[0034][0035]
由于所有约束均为定常约束，因此绳长变化速率与角速率的关系为：
[0036][0037]
其中，绳长变化率角速度
[0038]
进一步的，所述绳长变化速率与角速率求出后，根据搭载在基座上的单目全局相机辨识机械臂关节上的aruco码，通过aruco辨识方法测量关节角原理得到机械臂各个关节相对于零位的关节角度；实时测量关节角与零位标定角度做差得到关节角度偏差，该偏差经过pd控制器之后，可得到关节角速度期望；因为蛇形机械臂各个关节没有安装驱动电机，最后驱动方式需转化为绳索拉力进而拉动各个关节运动，因此需要利用到雅克比矩阵把得到的关节角速度转化为绳索速率的变化量，在通过积分器对速率求积分得到绳索绳长的变化；
[0039]
最后根据绳长速度变化驱动样机蛇形臂各个关节运动，基座单目相机实时测量机械臂关节角度，最终使各个关节角度回到零位位置，达到自主回零效果。
[0040]
本发明的有益效果是：
[0041]
1.本发明基于aruco辨识方法作为蛇形机械臂的关节角标定，可方便快捷地为蛇形机械臂每一关节提供唯一识别号机械臂关节id，方便于后续关节角计算。
[0042]
2.本发明使用闭环反馈可以实现蛇形臂运动过程中实时位置停下后自主回零，有利于减少机械臂上的电子测量元器件安装，适应更加复杂、恶劣环境。
附图说明
[0043]
图1本发明蛇形机械臂整机结构图。
[0044]
图2本发明相机配置示意图。
[0045]
图3本发明aruco粘贴位置示意图。
[0046]
图4本发明提取aruco码示意图。
[0047]
图5本发明回零方案流程图。
[0048]
图6本发明aruco标识码示意图。
具体实施方式
[0049]
下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0050]
一种蛇形机械臂视觉反馈与自主回零的方法，所述方法包括以下步骤：
[0051]
步骤1：通过aruco辨识方法测量关节角，利用相机外参数与关节角之间存在的关系，利用aruco关节角辨识方法测量蛇形机械臂每一个关节的实时关节角；所述相机外参数为矩阵t，t＝[r,t]，其中r为3
×
3的正交单位矩阵(也叫旋转矩阵)t为三维平移向量，矢量获得各个aruco标识码坐标系相对于相机坐标系的变换关系，在通过解算机械臂关节的id号，就能确定aruco标识码与关节号之间对应关系；
[0052]
步骤2：通过aruco辨识方法测量关节角后，测得的实时关节用于蛇形机械臂自主回零，如图5所示，由全局单目相机测得实时各个关节的关节角度，与角度期望值做差后作为角度误差输入进pd控制器，得到关节角速度，经雅克比矩阵得到绳索速度在通过积分器得到绳索变化量，进而构成自主回零闭环控制。蛇形机械臂工作过程中受环境影响，没有关节角测量电子元件，因此采用视觉标定的自主回零方法。
[0053]
进一步的，先求各个aruco标识码坐标系(o1,o2,o3,o4,o5)相对于基座坐标系的姿态变换矩阵
[0054][0055]
其中，t4为相机坐标系至基座坐标系的姿态变换矩阵，
[0056]
t4＝t2t
1t
t3ꢀꢀꢀ
(2)
[0057]
r
ic
为第i个aruco标识码相对于相机坐标系的旋转矩阵，是单目相机量测值。
[0058]
进一步的，所述步骤1中机械臂关节id号的计算，根据aruco标识码坐标系的轴与机械臂关节的z轴向平行解算关节角，轴在上一个关节的关节坐标系坐标系下的坐标分量n
iy
为，
[0059][0060]
其中，为第i关节aruco标识码坐标系至第i-1关节坐标系的姿态变换矩阵如下，
[0061][0062]
其中，r
i-1,0
为关节坐标系相对基座坐标系的旋转矩阵，绕x,y轴旋转的关节角α
i
，β
i
可以由下列方程求出，
[0063]
n
iy
＝rot
y
(β
i
)rot
x
(α
i
)[0 0 1]
t
ꢀꢀꢀ
(5)
[0064]
其中rot
x
(α)为绕x轴旋转α角度的转转矩阵，rot
y
(b)为绕y轴旋转β角度的旋转矩阵，r
i-1,0
可以由如下公式求得，
[0065]
r
i，0
＝r
i，i-1
r
i-1，i-2
…
r
1，0
r
0，0
ꢀꢀꢀ
(6)
[0066]
其中，均表示第i关节aruco标识码坐标系至第i-1关节坐标系的姿态变换矩阵，当计算第一关节相对于基座坐标系的变换矩阵时可以由算得，从i≥2时由计算，而后带入(4)求得
[0067]
最后由式(3)和式(5)联立即可求得，绕x轴旋转的关节角α和绕y轴旋转的关节角
β，α和β即为基于aruco辨识方法测量的关节角。
[0068]
进一步的，所述步骤2具体为，首先求得绳索长度与关节速度的雅克比矩阵设为j
t
，可通过绳索拉力与等效力矩之间的映射关系求得，
[0069][0070][0071]
其中，z0,z1,
…
z
2i-1
为d-h坐标系的z轴在惯性空间中的单位向量，p0,p1,
…
p
2i
为d-h坐标系的原点在惯性空间中的矢量坐标。
[0072]
进一步的，所述公式(8)变换成，
[0073][0074]
其中，驱动力在椎段i的体坐标系原点处产生的力旋量为
[0075][0076]
干扰力在椎段i的体坐标系原点处产生的力旋量为
[0077][0078]
由于所有约束均为定常约束，因此绳长变化速率与角速率的关系为：
[0079][0080]
其中，绳长变化率角速度
[0081]
进一步的，对于规划绳长速度将用于图5中自主回零方案。对于本套蛇形臂系统来讲，蛇形臂各个关节没有安装电子元器件，比如编码器，因此对于关节角度测量依靠视觉测量得到的关节角α和β；当蛇形机械臂运动到任意位置后，绳长变化速率与角速率求出后，根据搭载在基座上的单目全局相机辨识机械臂关节上的aruco码，通过aruco辨识方法测量关节角原理得到机械臂各个关节相对于零位的关节角度；具体回零方案如图5所示，实时测量关节角与零位标定角度做差得到关节角度偏差，该偏差经过pd控制器之后，可得到关节角速度期望；因为蛇形机械臂各个关节没有安装驱动电机，最后驱动方式需转化为绳索拉力进而拉动各个关节运动，因此需要利用到雅克比矩阵把得到的关节角速度转化为绳索速率的变化量，在通过积分器对速率求积分得到绳索绳长的变化；
[0082]
最后根据绳长速度变化驱动样机蛇形臂各个关节运动，基座单目相机实时测量机械臂关节角度，进而形成图5闭环方案，最终使各个关节角度回到零位位置，达到自主回零
效果。