[发明专利]一种基于刚体变换的DH推导方法

权利要求书

1.一种基于刚体变换的DH推导方法，其特征在于，包括以下步骤：

步骤1：将机械臂所包含的6个关节角度记为θ₁、θ₂、θ₃、θ₄、θ₅、θ₆,6个关节对应的连杆长度为固定长度，整个机械臂的运动由6个坐标变换构成，第i坐标变换矩阵由对应θ_i决定，θ_i表示两相邻连杆绕公共轴线的夹角；

步骤2：使用仿真软件计算机械臂的关节长度、位置设定角度；建立DH连杆参数表；

步骤3：DH推导转换，式中，表示第i-1关节的坐标到第i关节的坐标的齐次变换，表示绕x_i-1轴旋转a_i-1(deg)角，a_i-1(deg)表示两个相邻关节轴的夹角，表示绕Z_i轴旋转θ_i角，表示沿x_i-1轴平移距离a_i-1(mm)，a_i-1(mm)表示两个相邻关节轴公垂线的长度，表示沿Z轴平移距离d_i,d_i表示沿两个相邻连杆公共轴线方向的距离,由旋转矩阵和平移向量求出机械臂位姿；

步骤4：由末端姿态推算出关节角度，根据DH变换，获得其中包含了参数θ₁，θ₂，θ₃，…θ_n，根据坐标末端执行姿态获得相对于基座坐标系o₀x₀y₀z₀的机械臂总的刚体变换矩阵记为记R由机械臂末端指向向量生成，p由机械臂末端坐标决定，在逆解问题中，R和P都为已知，另外基座坐标o₀x₀y₀z₀与J1坐标系o₁x₁y₁z₁定义出将从相对于基座的坐标系变换到相对于J1的坐标系，即由DH变换推导出来的包含参数的θ₁，…θ_n的与根据末端姿态参数获得的完全相等，即：

步骤5：角度推导，如果向量在x0y0z0坐标系中的经度记为β，纬度记为α，将变换到

根据相似变换，

记c_β＝k_z，

代入获取v_θ＝versθ＝1-c_θ，

对于给定的旋转

R_K,θ＝R_-K,-θ；

步骤6：角度求解。

2.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤1中，机械臂末端坐标系相对基座坐标系的变换矩阵H＝A₁(θ₁)...A_n(θ_n)，单杆位姿表示：

3.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤三中，基于

获取：

4.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤四，末端的位置定义了刚体变换矩阵中矩阵中的位移分量，末端的指向向量定义了刚体变换矩阵中的旋转矩阵。

5.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤5中，向量的长度是1，定义一个新的向量rvec＝(θk_x,θk_y,θk_z)^T,即该向量的方向与一样，角度为θ，向量rvec三个则全部表达了三维坐标系的所有参数，并且，所述θ不等于0。

6.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤6包括以下步骤：

步骤6.1，求解θ₁，θ₅，θ₆：

基本等式的(2,3)元素相等c₁z₂-s₁z₁＝-c₅

基本等式的(2,4)元素相等c₁p₂-p₁s₁＝-d₂-d₄-c₅d₆

整理消去c₅,获取s₁(p₁-z₁d₆)+c1(z₂d₆-p₂)＝d₂+d₄

记A＝p₁-z₁d₆；B＝z₂d₆-p₂；r²＝A²+B²

获取

将θ₁代入s₁(p₁-z₁d₆)+c1(z₂d₆-p₂)＝d₂+d₄

获取

基本等式的(2,1)元素相等c₁x₂-s₁x₁＝c₆s₅

基本等式的(2,2)元素相等c₁y₂-s₁y₁＝-s₅s₆

获取θ₆＝atan2(±(s₁y₁-c₁y₂),±(c₁y₂-s₁x₁))；

步骤6.2，求解θ₃，θ₂：

由等式的(1,4)元素相等。

c₁p₁+p₂s₁-d₅(c₆(c₁y₁+s₁y₂)+s₆(c₁x₁+s₁x₂))-d₆(c₁z₁+s₁z₂)＝a₃c₂+a₄(c₂c₃-s₂s₃)等

式的(1,4)元素相等：

p₃-d₁-d₆z₃-d₅(c₆y₃+s₆x₃)＝a₃s₂+a₄(c₂s₃+c₃s₂)

X＝c₁p₁+p₂s₁-d₅(c₆(c₁y₁+s₁y₂)+s₆(c₁x₁+s₁x₂))-d₆(c₁z₁+s₁z₂)

Y＝p₃-d₁-d₆z₃-d₅(c₆y₃+s₆x₃)

推出：

a₃c₂+a₄c₃＝X，

a₃s₂+a₄s₃＝Y

则

当上式中D的值不介于[-1,1],说明该解无效，需要舍弃；

将θ₃带入a₃c₂+a₄c₃＝X，a₃s₂+a₄s₃＝Y，获取E＝(a₃+a₄c₃)Y-a₄s₃X，F＝(a₃+a₄c₃)X+a₄s₃Y，获取θ₂＝atan2(E,F)；

步骤6.3，求解θ₄：

由等式的(1,3)元素相等；

z₃(c₂c₃+c₃s₂)+c₁z₁(c₂c₃-s₂s₃)+s₁z₂(c₂c₃-s₂s₃)＝c₄s₅，等式的(2,3)元素相等；

z₃(c₂c₃-s₂s₃)-c₁z₁(c₂s₃+c₃s₂)-s₁z₂(c₂c₃+c₃s₂)＝s₄s₅

记：M＝z₃(c₂c₃-s₂s₃)-c₁z₁(c₂s₃+c₃s₂)-s₁z₂(c₂s₃+c₃s₂)

N＝z₃(c₂s₃+c₃s₂)+c₁z₁(c₂c₃-s₂s₃)+s₁z₂(c₂c₃-s₂s₃)

获取θ₄＝atan2(±M,±N)。