[发明专利]一种多任务机械臂位姿检测和误差补偿方法

权利要求书

1.一种多任务机械臂位姿检测和误差补偿方法，其特征在于，包括以下步骤：

1)标定双目立体视觉传感器，建立机械臂的D-H运动模型，得到D-H运动模型的各坐标转换矩阵^jT₆(j＝0～5)的理论表达式；

2)设置机械臂的任务类型，根据该任务类型及工作环境，系统自动生成位姿补偿的权重系数w_i(i＝1～6)；

3)给定目标位姿，求取各关节的理论角度q_t＝[q₁,q₂,q₃.q₄,q₅,q₆]^T，并设置初始误差补偿量Δq＝[0,0,0，0,0,0]^T；

4)利用双目立体视觉的方法，测量机械臂末端的位姿信息，计算末端位姿的综合误差D＝[d_x,d_y,d_z,δ_x,δ_y,δ_z]^T，其中，d_x,d_y,d_z为位置误差，δ_x,δ_y,δ_z为姿态误差；

5)实时求取各坐标转换矩阵，得到雅克比矩阵J(q)；

6)求取各关节的新增误差补偿量Δq_add，则新的误差补偿量为Δq＝Δq+Δq_add；

7)给定下一点的目标位姿，求取各关节的理论角度q_t＝[q₁,q₂,q₃.q₄,q₅,q₆]^T，并设置各关节实际运动角度为q＝q_t-Δq进行运动；

8)重复步骤4)～7)，直至完成任务。

2.根据权利要求1所述的多任务机械臂位姿检测和误差补偿方法，其特征在于，步骤1)中，假设从第k-1个关节(第0个关节为基座)到第k个关节的坐标变换为：

上式中，关节转角θ_i表示X_i-1轴绕Z_i-1轴转动到X_i轴所需旋转的角度；关节偏置d_i表示X_i-1轴沿着Z_i-1轴移动到X_i轴的距离；连杆扭角α_i表示Z_i-1轴绕X_i轴转动到Z_i轴所需旋转的角度；连杆长度a_i表示Z_i-1轴沿着X_i轴转动到Z_i轴所需旋转的角度；

则机械臂末端的坐标系与连杆j-1坐标系的转换矩阵^jT₆(j＝0～5)的理论表达式为：

其中，n_x、n_y、n_z、o_x、o_y、o_z、a_x、a_y、a_z、p_x、p_y、p_z分别表示^jT₆前三行中12个元素对应的数值。

3.根据权利要求1所述的多任务机械臂位姿检测和误差补偿方法，其特征在于，步骤3)中，初始误差补偿量Δq＝[0,0,0,0,0,0]^T。

4.根据权利要求1所述的多任务机械臂位姿检测和误差补偿方法，其特征在于，步骤5)中，雅克比矩阵J(q)的第m列由^mT₆决定，可表示为：

其中，n,o,a和p是^mT₆的四个列向量。