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

本发明专利技术公开了一种蛇形机械臂视觉反馈与自主回零的方法。步骤1：通过ArUco辨识方法测量关节角，利用相机外参数与关节角之间存在的关系，利用ArUco关节角辨识方法测量蛇形机械臂每一个关节的实时关节角；步骤2：通过ArUco辨识方法测量关节角后，测得的实时关节用于蛇形机械臂自主回零。本发明专利技术解决传统蛇形机械臂任意位置回零困难的问题，进而提出基于ArUco标识的蛇形机械臂关节角辨识方法，能够测量机械臂关节角，从而用于蛇形臂的自主回零过程。从而用于蛇形臂的自主回零过程。从而用于蛇形臂的自主回零过程。

【技术实现步骤摘要】
一种蛇形机械臂视觉反馈与自主回零的方法


[0001]本专利技术属于蛇形机械臂的
；具体涉及一种蛇形机械臂视觉反馈与自主回零的方法。

技术介绍

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

技术实现思路

[0005]本专利技术解决传统蛇形机械臂任意位置回零困难的问题，进而提出基于ArUco标识的蛇形机械臂关节角辨识方法，能够测量机械臂关节角，从而用于蛇形臂的自主回零过程。
[0006]本专利技术通过以下技术方案实现：
[0007]一种蛇形机械臂视觉反馈与自主回零的方法，所述方法包括以下步骤：
[0008]步骤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]下面将结合本专利技术实施例中的附图对本专利技术实施例中的技术方本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种蛇形机械臂视觉反馈与自主回零的方法，其特征在于，所述方法包括以下步骤：步骤1：通过ArUco辨识方法测量关节角，利用相机外参数与关节角之间存在的关系，利用ArUco关节角辨识方法测量蛇形机械臂每一个关节的实时关节角；所述相机外参数为矩阵T，T＝[R,t]，其中R为3
×
3的正交单位矩阵t为三维平移向量，矢量获得各个ArUco标识码坐标系相对于相机坐标系的变换关系，在通过解算机械臂关节的ID号，就能确定ArUco标识码与关节号之间对应关系；步骤2：通过ArUco辨识方法测量关节角后，测得的实时关节用于蛇形机械臂自主回零，由全局单目相机测得实时各个关节的关节角度，与角度期望值做差后作为角度误差输入进PD控制器，得到关节角速度，经雅克比矩阵得到绳索速度在通过积分器得到绳索变化量，进而构成自主回零闭环控制。2.根据权利要求1所述一种蛇形机械臂视觉反馈与自主回零的方法，其特征在于，先求各个ArUco标识码坐标系(O1,O2,O3,O4,O5)相对于基座坐标系的姿态变换矩阵各个ArUco标识码坐标系(O1,O2,O3,O4,O5)相对于基座坐标系的姿态变换矩阵其中，T4为相机坐标系至基座坐标系的姿态变换矩阵，T4＝T2T
1T
T
3 (2)R
ic
为第i个ArUco标识码相对于相机坐标系的旋转矩阵，是单目相机量测值。3.根据权利要求1所述一种蛇形机械臂视觉反馈与自主回零的方法，其特征在于，所述步骤1中机械臂关节ID号的计算，根据ArUco标识码坐标系的O
i
Y轴与机械臂关节的Z轴向平行解算关节角，O
i
Y轴在上一个关节的关节坐标系坐标系下的坐标分量n
iy
为，其中，为第i关节ArUco标识码坐标系至第i-1关节坐标系的姿态变换矩阵如下，其中，R
i-1,0
为关节坐标系相对基座坐标系的旋转矩阵，绕x,y轴旋转的关节角α
i
，β
i
可以由下列方程求出，n
iy
＝rot
y
(β
i
)rot
x
(α
i
)[0 0 1]
T
ꢀꢀ
(5)其中rot
x
(α)为绕x轴旋转...

【专利技术属性】
技术研发人员：魏承，张泽铭，谷海宇，姚程，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：