基于视觉识别的二维码定位抓取机器人系统算法的制作方法

本发明涉及多自由度机器人控制算法领域，特别涉及一种基于视觉识别二维码的全向移动平台的移动定位与四自由度机械臂抓取算法。

背景技术：

基于视觉的移动机器人自主导航是指移动机器人平台的图像采集系统获取机器人当前状态下的环境信息,通过对环境图像分析确定环境对象和机器人在环境中的位置。由于图像有较高分辨率,环境信息完整,符合人类的认知习惯,因此，近些年来，基于视觉的移动机器人自主导航被得到广泛关注和认可，并且在理论和实践方面已取得相当多的成果。现有技术中，已经存在有基于二维码实现移动小车定位的算法，用于控制移动小车向目标二维码移动。另外，在现有技术中，研究者将机器视觉和机械臂结合起来，为机械臂增加了智能的“眼睛”，可以大大增加机械臂的环境感知能力和智能决策能力，使机械臂可以轻松实现物品的抓取和放置等动作。但是目前未发现有将机械臂与移动平台相结合的视觉识别抓取算法；

因此，需要一种基于视觉识别的二维码定位抓取机器人系统算法，该算法结合pid算法的移动平台定位算法和机械臂逆运动学求解，提升了机械臂逆解的求解效率，较传统基座固定的机械臂，结合使用机械臂与移动平台，增加机械臂的操作范围和动作灵活性，采用二维码定位，提升抓取成功率。

技术实现要素：

有鉴于此，本发明针对现有技术的缺陷，提供了一种基于视觉识别的二维码定位抓取机器人系统算法，该算法实现基于视觉识别二维码的移动定位抓取动作，得以快速完成远处目标的抓取，增大传统基座固定机械臂的抓取范围，提升机器人的灵活性。

本发明的基于视觉识别的二维码定位抓取机器人系统算法，包括以下步骤：

s1.识别设有二维码的被抓取物体；s2.提取所述二维码几何中心的位姿信息；s3.设定到所述二维码相对位姿静止的目标值；计算当前时刻二维码位姿与设定的位姿目标之间的偏差，并将该偏差与设定的阈值做比较；若偏差超过阈值，则结合pid算法，将偏差信息转化为速度信息，并根据该速度信息控制麦轮平台运动到位姿目标后静止；若偏差未超过阈值，则保持麦轮平台静止；s4.利用所述二维码位姿信息进行机械臂的逆运动学求解，使机械臂末端机械手的中心夹持位置达到二维码中心；经过坐标变换，求解出机械臂驱动电机的转角；驱动电机输出该转角带动机械臂末端执行器运动到被抓取物体后完成夹持动作；

进一步的，步骤s2中，采用开源二维码识别算法软件包ar_track_alvar或visp，提取二维码几何中心的位姿信息[xcurrentycurrentzcurrent]^t以及四元素[qxcurrentqycurrentqzcurrentqwcurrent]^t；

进一步的，步骤s3包括：

s31.通过所述四元素[qxcurrentqycurrentqzcurrentqwcurrent]^t得到二维码几何中心的偏航角yawcurrent，

s32.设定[xgoalygoalyawgoal]^t为相机与二维码之间的目标位姿；设定[xthresholdythresholdyawthreshold]^t为当前二维码位姿与设定的目标位姿之间的差值临界阈值；设定[xoffsetyoffsetyawoffset]^t(t)为当前时刻二维码位姿与设定的目标位姿之间的偏差；设定[vxvywz]^t(t)为相机坐标系ocameraxyz下，当前时刻麦轮移动平台几何中心的速度；

s33.计算当前时刻二维码位姿与设定的目标位姿之间的偏差[xoffsetyoffsetyawoffset]^t(t),其计算公式为：

s34.若当前时刻二维码位姿与设定的目标位姿之间的偏差[xoffsetyoffsetyawoffset]^t(t)超过所述阈值[xoffsetyoffsetyawoffset]^t(t)，则根据麦轮移动平台几何中心的速度[vxvywz]^t(t)控制麦轮平台运动，结合pid算法，以加快麦轮平台向目标点移动，所述麦轮移动平台几何中心的速度[vxvywz]^t(t)的计算公式如下

若当前时刻二维码位姿与设定的目标位姿之间的偏差[xoffsetyoffsetyawoffset]^t(t)小于所述阈值[xoffsetyoffsetyawoffset]^t(t)，则麦轮平台保持静止状态，即

s35.将相机坐标系ocameraxyz的速度信息变换到麦轮平台坐标系omecanum_basexyz，变换公式为：

其中，表示相机坐标系下ocameraxyz求得的速度；

表示麦轮平台坐标系下omecanum_basexyz，麦轮平台运动中心的速度。

s36.根据得到的麦轮平台几何中心的速度信息转换为每个麦克纳姆轮的角速度[w0w1w2w3]^t，转换公式为：

其中，a为麦轮平台的宽度，b为麦轮平台的长度，r为麦轮大轮的半径。

s37.将求解出的角速度[w0w1w2w3]^t分别传送给控制麦轮的电机，驱动麦轮平台的运动；

进一步的，步骤s4包括：

s41.求解h点在相机坐标系ocameraxyz的位置，求解公式为：

^camerah＝[^cameraxcurrent-lgh^cameraycurrent^camerazcurrent]^t；

求解h点在机械臂底座旋转坐标系oarm_basexyz的位置：

机械臂局部坐标系mlocalxyz的x轴与机械臂底座旋转坐标系oarm_basexyz的y轴的夹角

s42.求解点h在机械臂局部坐标系mlocalxyz的位置:

^localhy＝0；

^localhz＝^arm_basehz-la_l；

其中，da_l为机械臂底座旋转坐标系oarm_basexyz与局部坐标系mlocalxyz两者z轴之间的距离；

la_l为机械臂底座旋转坐标系oarm_basexyz与局部坐标系mlocalxyz两者的x0y平面在z轴方向上的距离；

s43.求解出l1在机械臂局部坐标系mlocalxyz与x轴的夹角.1，

求解出l2在机械臂局部坐标系mlocalxyz与x轴的夹角.2，

其中，

本发明的有益效果：采用本发明的基于视觉识别的二维码定位抓取机器人系统算法通过移动平台与机械臂之间的协同配合，增加机械臂抓取的范围、效率以及灵活性，同时较之工业型机械臂成本更低。

附图说明

下面结合附图和实施例对本发明作进一步描述。

图1为本发明的移动抓取机器人结构示意图；

图2为本发明的二维码在相机坐标系下的位姿示意图；

图3为本发明的相机-麦轮平台坐标关系示意图；

图4为本发明的麦轮平台结构简图；

图5为本发明的机械臂机构简图；

图6为本发明中机械臂底座旋转坐标系oarm_basexyz及其局部坐标系mlocalxyz关系示意图；

图7为本发明的机械臂局部逆运动学计算简图；

图8为本发明的算法流程图。

具体实施方式

1移动抓取机器人结构示意图

如图1所示，机器人通过固定于麦轮平台上的相机获取二维码图像，并通过相关识别算法提取位姿信息，进而控制麦轮平台的移动和机械臂的运动，最终完成贴有二维码标签的物体的抓取任务。图1中主要包含3部分：1.麦克纳姆轮全向移动平台(后文简称麦轮平台)；2.相机；3.四自由度机械臂。

2二维码位姿矩阵获取

通过ar_track_alvar或visp开源算法包，可以实时计算出二维码的位姿矩阵，具体如下：如图2所示，

[xcurrentycurrentzcurrent]^t：二维码几何中心在相机坐标系下的空间位置；

[qxcurrentqycurrentqzcurrentqwcurrent]^t：二维码几何中心在相机坐标系下的四元素(角度)。

3麦轮平台移动定位

当麦轮平台距离二维码较远时，需要将二者的相对位置关系转化成速度信息，以控制麦轮平台移动至相对二维码的目标位置。

通过上述开源算法包提取的二维码几何中心姿态信息，通过四元素转化为二维码几何中心的偏航角yawcurrent，计算公式如下：

通过二维码相对相机位姿参数[xyyaw]^t，将其转化为速度信息发送给麦轮移动平台，如图3，具体转化过程如下：

设定：[xgoalygoalyawgoal]^t：表示相机与二维码之间的最终达到平衡(静止)的位姿目标值(需要在程序中设定合理的值)；

[xthresholdythresholdyawthreshold]^t：表示当前二维码位姿与设定的位姿目标值之间差值临界阈值(需要在程序中设定合理的值)；

[xoffsetyoffsetyawoffset]^t(t)：表示当前时刻二维码位姿与设定的位姿目标值之间的偏差；

[vxvywz]^t(t)：相机坐标系下ocameraxyz，当前时刻麦轮移动平台几何中心的速度；

计算出t时刻，目标值与当前值(二维码当前位姿)的偏差，

即：

如果偏差超出设定的阈值(麦轮平台距离二维码太远)，

即：

则控制麦轮平台运动，结合pid算法，以加快麦轮平台向目标点移动，计算公式如下(通过调节pid参数，以达到最佳效果):

否则麦轮平台保持静止状态，

即：

相机坐标系ocameraxyz与麦轮平台坐标系omecanum_basexyz不同，需要进行坐标变换，将相机坐标系ocameraxyz的速度信息转换到麦轮平台坐标系omecanum_basexyz，如下

表示相机坐标系下ocameraxyz求得的速度

表示麦轮平台坐标系下omecanum_basexyz，麦轮平台运动中心的速度

二者之间的速度信息的坐标变换为

4麦轮平台逆运动学方程

将麦轮平台移动定位中求解得到的麦轮平台几何中心的速度信息转换为每个麦克纳姆轮的角速度，具体过程如下

如图4所示，麦轮平台按照“内八”方式排布四个麦轮，建立omecanum_basexyz坐标系，逆时针定义四个麦轮，标号为0、1、2、3，平台几何中心omecanum_base的运动速度表示为：

[vcw]^t

vc：几何中心omecanum_base的线速度；

w：几何中心omecanum_base的角速度；

a：麦轮平台的宽度；

b：麦轮平台的长度；

r：麦轮大轮的半径；

[v0v1v2v3]：分别代表四个麦轮的速度矢量。

从图4可知，通过速度分解方法，单个麦轮的速度通用公式可由以下计算：

进一步求解单个麦轮的转动角速度：

在t时刻的速度求解方程为：

将上式中求解出的角速度[w0w1w2w3]^t分别传送给控制麦轮的电机，实现麦轮平台的运动。

5四自由度机械臂逆运动学求解

在麦轮平台运动到目标位置([xgoalygoalyawgoal]^t)后，便控制机械臂抓取贴有二维码标签的物体，具体抓取算法如下：

机械臂结构采用开源的dobot和uarm机械臂的四自由度(包含机械手爪)的结构，具有运动精度高、负载能力强等优点，但目前存在的文献中，几乎都采用dh法来计算机械臂的逆运动学方程，求解过程速度较慢、方法较复杂，因而本算法巧妙利用其结构的几何约束关系，求解机械臂三个关节的运动角度。

如图5所示，包含三个坐标系：相机坐标系ocameraxyz、机械臂底座旋转坐标系oarm_basexyz和机械臂局部坐标系mlocalxyz，g表示机械手爪的“虎口”中心，也即是二维码几何中心所在位置，机械手爪hg始终保持水平姿态(几何约束：α1＝α2+α3)，机械臂的结构参数、上述三个坐标系之间的相对位置关系以及g点在相机坐标系ocameraxyz的位置是已知的，以求解出运动学逆解[θ1θ2θ3]^t。

各参数如下(结合图5和图6)：

θ1：表示l1在机械臂局部坐标系mlocalxyz与x轴的夹角；

θ2：表示l2在机械臂局部坐标系mlocalxyz与x轴的夹角；

θ3：表示机械臂局部坐标系mlocalxyz的x轴与机械臂底座旋转坐标系oarm_basexyz的y轴的夹角(如图6)。

具体机械臂逆解计算过程和如下:

1.已知末端机械手的中心夹持位置为g(也就是二维码中心位置)在相机坐标系ocameraxyz下的坐标为^camerag＝[^cameraxcurrent^cameraycurrent^camerazcurrent]^t，各个连杆的长度表示为l1、l5、l6，求解两舵机转角[θ1θ2]^t和底座步进电机转角θ3；

2.由于相机坐标系ocameraxyz、机械臂底座旋转坐标系oarm_basexyz和机械臂局部坐标系mlocalxyz(如图5所示),三个坐标系之间的平移变换关系为：

1)相机坐标系ocameraxyz相对于机械臂底座旋转坐标系oarm_basexyz的位置关系；

2)如图5和图6所示，机械臂底座旋转坐标系oarm_basexyz与机械臂局部坐标系mlocalxyz之间的关系

3.底座步进电机转角θ3

如图5和图6所示，首先计算出h点在相机坐标系ocameraxyz下的位置，再计算出h点在机械臂底座旋转坐标系oarm_basexyz下的空间位置，最后投影到oarm_basexyz平面做计算，具体过程如下：

求h点在相机坐标系ocameraxyz的位置：

lgh＝l5cosα3

^camerah＝[^cameraxcurrent-lgh^cameraycurrent^camerazcurrent]^t

求h点在机械臂底座旋转坐标系oarm_basexyz的位置：

从图6中得出

4.求解转角θ1和θ2

1)如图6所示，求解点h在机械臂局部坐标系mlocalxyz的位置

^localhy＝0

^localhz＝^arm_basehz-la_l

da_l：表示机械臂底座旋转坐标系oarm_basexyz与局部坐标系mlocalxyz两者z轴之间的距离

la_l：表示机械臂底座旋转坐标系oarm_basexyz与局部坐标系mlocalxyz两者xoy平面在z轴方向上的距离；

2)如图7所示，

3)计算δθ＝θ2-θ1，

在δmhg中，由余弦定理得，

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。