[发明专利]一种智能捡球机器人及其机械臂控制方法

权利要求书

1.一种智能捡球机器人，其特征在于，包括检测装置、移动装置、拾取装置和电能储存装置；

检测装置、拾取装置和电能储存装置都设置在移动装置上；

检测装置包括摄像头，通过支杆设置在移动装置上，摄像头倾斜正对移动装置的正前方；

移动装置上设置有直流电机和电机驱动板，通过电机驱动板进行PWM调速控制直流电机转动，控制移动装置移动；

拾取装置包括机械臂底座、机械臂和舵机驱动板，舵机驱动板用于驱动机械臂的运动，舵机驱动板设置在距离机械臂底座处，通过运动规划使机械臂末端到达空间中的某个位置；

机械臂包括舵机和机械手，舵机包括第一舵机、第二舵机、第三舵机、第四舵机和第五舵机；所述第一舵机固定设置在机械臂底座上，第一舵机与第二舵机通过第一支架相连接；第二舵机与第三舵机通过第二支架相连接，第三舵机与第四舵机通过第三支架相连接，第四舵机与第五舵机通过第四支架相连接，第五舵机连接机械手；

第一舵机用于控制方向，第五舵机控制机械手的开合，第二舵机、第三舵机和第四舵机用于控制旋转角度；

进行捡球具体包括以下步骤：

S1，采集目标物体色块的阈值；

S2，通过图像处理得到目标物体色块的位置信息，获取目标物体色块的中心点坐标，目标物体色块中心与机械臂底座之间的距离a，判断机械臂是否可达，机械臂不可达，进入步骤S3，机械臂可达，进入步骤S4；

S3，通过舵机驱动板控制电机驱动板，电机驱动板收到舵机驱动板发出的指令进行PWM调速控制电机转动快慢，控制移动装置前进、后退或者转弯；

S4，驱动移动装置与机械臂的舵机驱动板上接收到了关于物体色块的位置信息，根据所述位置信息，计算机械臂的舵机转动的角度和电机转动时间；

步骤S2具体包括以下步骤：

对识别到的色块获取色块中心点的坐标(blob.cx,blob.cy)，将目标物体色块中心的y坐标blob.cy标记为y,y＝blob.cy；

目标物体色块中心与机械臂底座之间的距离a通过摄像头获取到的目标物体色块中心的y坐标值计算；

选取一个用于参考的y坐标，将参考目标物体色块放在机械臂正前方能抓取到的位置，记录此时的y坐标，记为y_r，机械臂能抓取到的最近目标物体色块的中心坐标y_max＝y_r+f，能抓取到的最远目标物体色块的中心坐标y_min＝y_r-e，当y_ty_r+f时或者y_ty_r-e时，机械臂不可达，驱动移动装置移动，否则，机械臂可达，进入步骤S4，y_t为当前目标物体色块的y坐标值，f为机械臂能抓取到的最近目标物体色块的y坐标与参考目标物体色块的y坐标的最大增量，e为能抓取到的最远目标物体色块的y坐标与参考目标物体色块的y坐标的最大增量的绝对值；

步骤S3具体包括以下步骤：控制电机驱动板上的逻辑输入引脚IN1和IN2输入高电平1或低电平0，确定电机的转动方向，输出PWM脉冲，计算驱动直流电机转动时间，进行直流电机调速；

计算驱动直流电机转动时间具体包括以下步骤：

当y_ty_r-e，

目标物体色块位于机械臂够不到的位置，此时驱动直流电机正转时间为：(y_r-e-y)*h(ms)，h为电机转动系数；

当y_r-e≤y_t≤y_r+f

目标物体色块处于能被机械臂抓取到的位置，直流电机不转动，直接控制机械臂，进入步骤S4；

当y_ty_r+f

目标物体色块距离机械臂过近，此时驱动直流电机反转时间为：(y_r+f-y)*h(ms)；

步骤S4具体包括以下步骤：

获取的目标物体色块的中心到机械臂底座距离为a，第四舵机距离地面高度为b；

进行舵机转动角度计算，然后驱动机械臂完成捡球动作

舵机转动角度θ计算公式为：

L₀+c+L₁cosθ₁＝b+L₂sin[θ₂-(90°-θ₁)]

L₀+c+L₁cosθ₁＝b-L₂cos(θ₁+θ₂) ①

L₁sinθ₁+L₂cos[θ₂-(90°-θ₁)]+L₃＝a—L₄

L₃+L₁sinθ₁+L₂sin(θ₁+θ₂)＝a—L₄ ②

θ₁、θ₂和θ₃以连接舵机的前一连杆延长线为起始线进行的度量，顺时针转过为正，逆时针转过为负；

以L₀的延长线为起始线，第二舵机驱动第二舵机与第三舵机之间的大臂顺时针转过的角度为θ₁，第二舵机驱动第二舵机与第三舵机之间的大臂逆时针转过的角度为-θ₁；

L₀为第一舵机与第二舵机之间的连杆长度，θ₁为第二舵机旋转角度，第一舵机到地面的高度为c；

L₁为第二舵机与第三舵机之间大臂的长度，θ₂为第三舵机旋转角度；

L₂为第三舵机与第四舵机之间小臂的长度，θ₃为第四舵机旋转角度；

L₃为第四舵机与第五舵机之间的连杆长度，第四舵机到地面的高度为b；

L₄为第五舵机与目标物体色块的水平距离，θ₄为第五舵机旋转角度；

第一舵机转动角度为θ₀，第二舵机转动角度为θ₁，第三舵机转动角度为θ₂，第四舵机转动角度为θ₃，第五舵机转动角度为θ₄，控制机械手的开合；

联立①和②

L₀+c+L₁cosθ₁+L₂cos(θ₁+θ₂)＝b

L₃+L₁sinθ₁+L₂sin(θ₁+θ₂)＝a- L₄

则：

令

则：cosθ₂＝uθ₂＝arcosu

代入①式得：L₁cosθ₁+L₂cosθ₁cosθ₂-L₂sinθ₁sinθ₂＝b-L₀-c

整理得：

令则：

整理可得：X m²+Y m+Z＝0

X＝b-L₀-c+L₁+uL₂

Z＝b-L₀-c-L₁-uL₂

解得

90°-θ₁-θ₃＝θ₂