[发明专利]一种基于双目立体视觉的SCARA机器人轨迹规划方法

权利要求书

1.一种基于双目立体视觉的SCARA机器人轨迹规划方法，其特征在于，包括以下步骤；

Step1、安装工业相机，获取工作区域图片

将两个相机安装在机器人工作区域的正上方，调整相机的焦距使整个工作区域处在两个相机的视野范围内；

Step 2、进行双目立体相机的标定

通过两个相机对不同方位的标定板进行同时拍照，并将每一次的标定板的位置与标定板的状态都记录下来，然后寻找一个公共的平面，使得两个相机的光轴平行,确定了此平面后，每次采集到的左右相机的图像都要先变换到此平面，然后利用三角定位法根据相同物体在左右相机中的视差来计算物体的高度H₁和障碍物的高度H₂；

Step 3、障碍物和工件的位置的确定

第一步，对工作区域进行拍照，利用背景相减法，用左相机的图像与背景图像相减得到只含有工件和障碍物的图像；

第二步，利用模板匹配的方法确定工件的位置，在背景相减后的图像中搜索模板图像，即将制作好的模板在当前图像上进行滑动和转动，用相似性度量来算出每块图像与模板图像的相似度，最终当相似值大于0.75时，认为匹配到模板，同时记录下当前坐标为匹配到工件的坐标，图片中去掉工件的图像，就可以确定障碍物的位置；

Step 4、确定轨迹插值点，轨迹的起点为工件坐标位置(X₀，Y₀)，终点为装配位置(X_f，Y_f)，为了绕开障碍物增加插值点(X_c，Y_c)；

Step 5、对SCARA机器人进行建模

计算机器人末端执行器与各个关节角度的转换关系，利用DH方法对机器人进行建模，机器人末端执行器的坐标为(Px,Py,Pz)，机器人第一和第二关节转过的角度分别为θ₁与θ₂，第一关节与第二关节的长度分别为L₁和L₂，第三个关节上下移动的距离为d₃；⁰T₄表示O₃坐标系相对于O₀坐标系的转换关系可由坐标系的转换公式得到公式1，最后一列与机器人坐标系的对应关系如公式2所示，即已知一点的机器人坐标(Px,Py,Pz)就可得到对应各个关节旋转的角度；

T04=T01T12T23T34=C12-4S12-40L2C12+L1C1S12-4-C12-40L2S12+L1S100-1-d30001---(1)]]>

Px=L2Cos(θ1+θ2)+L1Cosθ1Py=L2Sin(θ1+θ2)+L1Sinθ1Pz=-d3---(2)]]>

Step 6、对机器人进行关节空间的轨迹规划

采用五次多项式对轨迹进行拟合，其表达式如公式3所示，如公式4所示，计算起点的角度、速度和加速度分别为θ₀、和计算终点的角度、速度和加速度分别为θ_f、和轨迹规划要保证轨迹的初始速度和加速度都为零，终点的速度与加速度也为零，即起点的角度θ₀和终点的角度θ_f都已知，即可解方程得到关节的运动曲线方程如公式5所示，以此类推，其他关节运动的轨迹也可得到，这样就完成了机器人的轨迹规划，保证了机器人各个关节运动的速度和加速度都是连续可导的；

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5θ·(t)=a1+2a2t+3a3t2+4a4t3+5a5t4θ··(t)=2a2+6a3t+12a4t2+20a5t3---(3)]]>

θ0=a0θf=a0+a1tf+a2tf2+a3tf3+a4tf4+a5tf5θ·0=a1θ·f=a1+2a2tf+3a3tf2+4a4tf3+5a5tf4θ··0=2a2θ··f=2a2+6a3tf+12a4tf2+20a5tf3---(4)]]>

θ(t)=θ0+10(θf-θ0)tf3t3+15(θ0-θf)tf4t4+6(θf-θ0)tf5t5θ·(t)=30(θf-θ0)tf3t2+60(θ0-θf)tf4t3+30(θf-θ0)tf5t4θ··(t)=60(θf-θ0)tf3t+180(θ0-θf)tf4t2+120(θf-θ0)tf5t3---(5).]]>