[发明专利]基于视觉识别的二维码定位抓取机器人系统的方法

权利要求书

1.一种基于视觉识别的二维码定位抓取机器人系统的方法，其特征在于，包括以下步骤：

s1.识别设有二维码的被抓取物体；

s2.提取所述二维码几何中心的位姿信息；

s3.设定到所述二维码相对位姿静止的目标值；计算当前时刻二维码位姿与设定的位姿目标之间的偏差，并将该偏差与设定的阈值做比较；若偏差超过阈值，则结合PID算法，将偏差信息转化为速度信息，并根据该速度信息控制麦轮平台运动到位姿目标后静止；若偏差未超过阈值，则保持麦轮平台静止；

s4.利用所述二维码位姿信息进行机械臂的逆运动学求解，使机械臂末端机械手的中心夹持位置达到二维码中心；经过坐标变换，求解出机械臂驱动电机的转角；驱动电机输出该转角带动机械臂末端执行器运动到被抓取物体后完成夹持动作；

步骤s2中，采用开源二维码识别算法软件包ar_track_alvar或visp，提取二维码几何中心的位姿信息[x_current y_current z_current]^T以及四元数[qx_current qy_current qz_current qw_current]^T；

步骤s3包括：

s31.通过所述四元数[qx_current qy_current qz_current qw_current]^T得到二维码几何中心的偏航角yaw_current，

s32.设定[x_goal y_goal yaw_goal]^T为相机与二维码之间的目标位姿；

设定[x_threshold y_threshold yaw_threshold]^T为当前二维码位姿与设定的目标位姿之间的差值临界阈值；

设定[x_offset y_offset yaw_offset]^T_(t)为当前时刻二维码位姿与设定的目标位姿之间的偏差；

设定[v_x v_y w_z]^T_(t)为相机坐标系O_cameraXYZ下，当前时刻麦轮移动平台几何中心的速度；

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

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

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

s35.将相机坐标系O_cameraXYZ的速度信息变换到麦轮平台坐标系O_{mecanum_base}XYZ，变换公式为：

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

表示麦轮平台坐标系下O_{mecanum_base}XYZ，麦轮平台运动中心的速度；

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

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

s37.将求解出的角速度[w₀ w₁ w₂ w₃]^T分别传送给控制麦轮的电机，驱动麦轮平台的运动。

2.根据权利要求1所述的基于视觉识别的二维码定位抓取机器人系统的方法，其特征在于，步骤s4包括：

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

^cameraH＝[^camerax_current-L_GH^cameray_current^cameraz_current]^T；

求解H点在机械臂底座旋转坐标系O_{arm_base}XYZ的位置：

机械臂局部坐标系M_localXYZ的X轴与机械臂底座旋转坐标系O_{arm_base}XYZ的y轴的夹角

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

^localH_y＝0；

^localH_z＝^arm_baseH_z-l_{a_l}；

其中，d_{a_l}为机械臂底座旋转坐标系O_{arm_base}XYZ与局部坐标系M_localXYZ两者Z轴之间的距离；

l_{a_l}为机械臂底座旋转坐标系O_{arm_base}XYZ与局部坐标系M_localXYZ两者的XOY平面在Z轴方向上的距离；

s43.求解出L₁在机械臂局部坐标系M_localXYZ与X轴的夹角θ₁，

求解出L₂在机械臂局部坐标系M_localXYZ与X轴的夹角θ₂，

其中，