[发明专利]基于空间向量法的工业机器人运动学参数获取及校验方法

权利要求书

1.一种基于空间向量法的工业机器人运动学参数获取及校验方法，其特征在于包括以下步骤：

步骤一、建立机器人各关节轴连杆的坐标系，并画出它们的延长线，在关节i上，沿轴线建立坐标轴z_i，正方向任意；做关节轴i和关节轴i+1的公垂线，公垂线与轴线交点为第i个坐标系原点，坐标轴x_i由坐标系i的原点沿公垂线由i指向i+1；坐标轴y_i根据右手法则确定；

步骤二、借助3D建模软件从工业机器人的3D模型中的每一个关节轴上各获取两个点的坐标，利用3D建模软件捕获关节轴的截面圆圆心来获得；每个关节轴上所提取两个点作为关键点，沿着机械臂基座到末端的顺序分别记为：

a_i＝(a_ix,a_iy,a_iz),b_i＝(b_ix,b_iy,b_iz) (1)

步骤三、求取连杆长度a_i，关键点坐标求各关节的轴线z_i方向向量，记关节轴i，i+1的公垂线为n_i＝z_i×z_i+1，得到连杆长度表达式

步骤四、求取连杆扭角α_i，sgn()为符号函数，则

步骤五、求取连杆夹角θ_i，记x_i轴的矢量为x_i＝sgn((a_i+1-a_i)·n_i)·n_i，则

步骤六、求取连杆距离d_i，由于d_i为沿z_i轴从x_i-1移动到x_i的距离，首先求出连杆坐标系{i-1}和{i}的原点坐标O_i-1和O_i；O_i为关节轴i和关节轴i+1的公垂线和关节轴i的交点，则设O_i＝a_i+m₁·z_i，公垂线与关节轴i+1的交点为O_i'＝a_i+1+m₂·z_i+1，求|O_i-O_i'|取最小时的m₁，代入O_i＝a_i+m₁·z_i解得O_i；根据解出的O_i-1和O_i的坐标，求得d_i，

步骤七、求取机器人齐次矩阵根据求解的机器人运动学参数，连杆长度a_i、连杆扭角α_i、连杆夹角θ_i以及连杆夹角θ_i，机器人齐次变换矩阵由相邻连杆坐标系间的齐次变换矩阵顺序连乘求得，则

其中，p＝[p_x,p_y,p_z]^T为末端坐标系原点在参考坐标系中的坐标，n＝[n_x,n_y,n_z]^T、o＝[o_x,o_y,o_z]^T、a＝[a_x,a_y,a_z]^T分别为末端坐标系的x、y、z轴在参考坐标系中的单位矢量；

步骤八、求取机器人齐次矩阵与实际机器人齐次矩阵之间的误差，用位置误差A和姿态误差B表征，则

姿态误差B用通用旋转变换的方法来描述，对应的姿态绕着一单位矢量f旋转β到达另一姿态，旋转角β即为姿态误差，记通用旋转变换矩阵为Rot(f,β)