CN115741672A - 一种基于刚体变换的dh推导方法 - Google Patents

一种基于刚体变换的dh推导方法 Download PDF

Info

Publication number
CN115741672A
CN115741672A CN202211294024.3A CN202211294024A CN115741672A CN 115741672 A CN115741672 A CN 115741672A CN 202211294024 A CN202211294024 A CN 202211294024A CN 115741672 A CN115741672 A CN 115741672A
Authority
CN
China
Prior art keywords
theta
equal
angle
joint
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202211294024.3A
Other languages
English (en)
Other versions
CN115741672B (zh
Inventor
叶君
王俊
任骊澄
阮祖桢
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Bangjiexing Medical Technology Co ltd
Original Assignee
Hangzhou Bangjiexing Medical Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Bangjiexing Medical Technology Co ltd filed Critical Hangzhou Bangjiexing Medical Technology Co ltd
Priority to CN202211294024.3A priority Critical patent/CN115741672B/zh
Publication of CN115741672A publication Critical patent/CN115741672A/zh
Application granted granted Critical
Publication of CN115741672B publication Critical patent/CN115741672B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种基于刚体变换的DH推导方法，包括步骤1：将机械臂所包含的6个关节角度记为θ1、θ2、θ3、θ4、θ5、θ6,6个关节对应的连杆长度为固定长度，整个机械臂的运动由6个坐标变换构成，第i坐标变换矩阵由对应θi决定，θi表示两相邻连杆绕公共轴线的夹角；步骤2：使用仿真软件计算机械臂的关节长度、位置设定角度；建立DH连杆参数表；步骤3：DH推导转换，
Figure DDA0003902453880000011
式中，
Figure DDA0003902453880000012
表示第i‑1关节的坐标到第i关节的坐标的齐次变换；通过本发明的实施，可以充分体现出真实的效果，对路径规划和读取位置信息都能准确定位，提前仿真路径和避障，精准完成导航任务，模型建立完成后就可进行机械臂平滑运动，起到了突出性的进步，具有一定的使用价值和推广价值。

Description

一种基于刚体变换的DH推导方法
技术领域
本发明涉及机械臂运动推导方法技术领域，尤其涉及一种基于刚体变换的DH推导方法。
背景技术
分析机器人运动学，首先需要建立机器人各关节处坐标系，进而推导出基座坐标系与末端坐标系之间的位姿坐标关系，同时根据所建立的全局坐标系分析机器人运动学的正逆解方程；机器人的机械臂控制载体是机器人各系统、关节的载体，决定机器人性能的因素所在，如果结构产生错误会直接影响后续运行学的各种分析；因此机器人正逆运动学的推导对于合理的建立机器人坐标系就尤为重要，同时影响着机器人建模的精准度，所以亟需合理、高效的推导方法提高机器人建模的精准度。
发明内容
针对现有技术的不足，本发明提供了一种基于刚体变换的DH推导方法，旨在构建机器人工具坐标系相对其基座坐标系的位置姿态与各个关节输入角度的关系，根据坐标位置信息进行转换角度运行或者根据角度信息转换坐标位置信息进行转换的目的。
为实现上述目的，本发明提供如下技术方案：一种基于刚体变换的DH推导方法，包括以下步骤：
步骤1：将机械臂所包含的6个关节角度记为θ1、θ2、θ3、θ4、θ5、θ6,6个关节对应的连杆长度为固定长度，整个机械臂的运动由6个坐标变换构成，第i坐标变换矩阵由对应θi决定，θi表示两相邻连杆绕公共轴线的夹角；
步骤2：使用仿真软件计算机械臂的关节长度、位置设定角度；建立DH连杆参数表；
步骤3：DH推导转换，
Figure BDA0003902453860000021
式中，
Figure BDA0003902453860000022
表示第i-1关节的坐标到第i关节的坐标的齐次变换，
Figure BDA0003902453860000023
表示绕xi-1轴旋转ai-1(deg)角，ai-1(deg)表示两个相邻关节轴的夹角，
Figure BDA0003902453860000024
表示绕Zi轴旋转θi角，
Figure BDA0003902453860000025
表示沿xi-1轴平移距离ai-1(mm)，ai-1(mm)表示两个相邻关节轴公垂线的长度，
Figure BDA0003902453860000026
表示沿Z轴平移距离di,di表示沿两个相邻连杆公共轴线方向的距离,由旋转矩阵和平移向量求出机械臂位姿；
步骤4：由末端姿态推算出关节角度，根据DH变换，获得
Figure BDA0003902453860000027
其中包含了参数θ1，θ2，θ3，...θn，根据坐标末端执行姿态获得相对于基座坐标系o0x0y0z0的机械臂总的刚体变换矩阵记为
Figure BDA0003902453860000028
记
Figure BDA0003902453860000029
R由机械臂末端指向向量生成，p由机械臂末端坐标决定，在逆解问题中，R和P都为已知，另外基座坐标o0x0y0z0与J1坐标系o1x1y1z1定义出
Figure BDA00039024538600000210
将
Figure BDA00039024538600000211
从相对于基座的坐标系变换到相对于J1的坐标系，即
Figure BDA00039024538600000212
由DH变换推导出来的包含参数的θ1，…θn的与根据末端姿态参数获得的
Figure BDA00039024538600000213
完全相等，即：
Figure BDA00039024538600000214
步骤5：角度推导，如果向量
Figure BDA00039024538600000215
在x0y0z0坐标系中的经度记为β，纬度记为α，将
Figure BDA00039024538600000216
变换到
Figure BDA00039024538600000217
根据相似变换，
Figure BDA00039024538600000218
记
Figure BDA00039024538600000219
cβ＝kz，
代入得到
Figure BDA00039024538600000220
vθ＝versθ＝1-cθ，对于给定的旋转
Figure BDA0003902453860000031
Figure BDA0003902453860000032
RK,θ＝R-K,-θ；
步骤6：角度求解。
进一步的，所述步骤1中，机械臂末端坐标系相对基座坐标系的变换矩阵
Figure BDA0003902453860000033
H＝A1(θ1)…An(θn)，单杆位姿表示：
Figure BDA0003902453860000034
3.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤三中，基于
Figure BDA0003902453860000035
Figure BDA0003902453860000036
获取：
Figure BDA0003902453860000037
进一步的，所述步骤四，末端的位置定义了刚体变换矩阵中矩阵中的位移分量，末端的指向向量定义了刚体变换矩阵中的旋转矩阵。
进一步的，所述步骤5中，向量
Figure BDA0003902453860000038
的长度是1，定义一个新的向量rvec＝(θkx,θky,θkz)T,即该向量的方向与
Figure BDA0003902453860000039
一样，角度为θ，向量rvec三个则全部表达了三维坐标系的所有参数，并且，所述θ不等于0。
进一步的，所述步骤6包括以下步骤：
步骤6.1，求解θ1，θ5，θ6：
基本等式的(2,3)元素相等c1z2-s1z1＝-c5
基本等式的(2,4)元素相等c1p2-p1s1＝-d2-d4-c5d6
整理消去c5,获取s1(p1-z1d6)+c1(z2d6-p2)＝d2+d4
记A＝p1-z1d6；B＝z2d6-p2；r2＝A2+B2
获取θ1＝atan2((d2+d4),
Figure BDA0003902453860000041
将θ1代入s1(p1-z1d6)+c1(z2d6-p2)＝d2+d4
获取
Figure BDA0003902453860000042
基本等式的(2,1)元素相等c1x2-s1x1＝c6s5
基本等式的(2,2)元素相等c1y2-s1y1＝-s5s6
获取θ6＝atan2(±(s1y1-c1y2),±(c1y2-s1x1))；
步骤6.2，求解θ3，θ2：
由
Figure BDA0003902453860000043
等式的(1,4)元素相等。
c1p1+p2s1-d5(c6(c1y1+s1y2)+s6(c1x1+s1x2))-d6(c1z1+s1z2)＝a3c2+a4(c2c3-s2s3)等式的(1,4)元素相等：
p3-d1-d6z3-d5(c6y3+s6x3)＝a3s2+a4(c2s3+c3s2)
X＝c1p1+p2s1-d5(c6(c1y1+s1y2)+s6(c1x1+s1x2))-d6(c1z1+s1z2)
Y＝p3-d1-d6z3-d5(c6y3+s6x3)
推出：
a3c2+a4c3＝X，
a3s2+a4s3＝Y
Figure BDA0003902453860000051
则
Figure BDA0003902453860000052
Figure BDA0003902453860000053
当上式中D的值不介于[-1,1],说明该解无效，需要舍弃；
将θ3带入a3c2+a4c3＝X，a3s2+a4s3＝Y，获取E＝(a3+a4c3)Y-a4s3 X，F＝(a3+a4c3)X+a4s3 Y，获取θ2＝atan2(E,F)；
步骤6.3，求解θ4：
由
Figure BDA0003902453860000054
等式的(1,3)元素相等；
z3(c2c3+c3s2)+c1z1(c2c3-s2s3)+s1z2(c2c3-s2s3)＝c4s5，等式的(2,3)元素相等；
z3(c2c3-s2s3)-c1z1(c2s3+c3s2)-s1z2(c2c3+c3s2)＝s4s5
记：M＝z3(c2c3-s2s3)-c1z1(c2s3+c3s2)-s1z2(c2s3+c3s2)
N＝z3(c2s3+c3s2)+c1z1(c2c3-s2s3)+s1z2(c2c3-s2s3)
获取θ4＝atan2(±M,±N)。
本发明的实质性效果：通过推导正逆解，误差极小，建立模型可以充分体现出真实的效果，对路径规划和读取位置信息也都能够准确定位，采用把位姿信息转换成关节角度，那么采用关节角度运动，机械臂可以弧形运动，会在路径规划都可以很好的避障，运动也比较流畅；采用逆推进行转换成机械臂的关节角度，提前仿真路径以及避障，在路径规划前知道当前的坐标位姿和末端坐标位姿，精准完成导航任务，模型建立完成后就可以进行机械臂运行，把要运行的关节角度信息输入，就可以进行平滑运动。
附图说明
图1为本发明的正推过程数据流程示意图。
图2为本发明的逆推过程流程示意图。
图3为本发明的机械臂模型示意图。
图4为本发明的机械臂模型输入关节角度后的示意图。
具体实施方式
如图1、2、3、4所示，一种基于刚体变换的DH推导方法，包括以下步骤：
步骤1：将机械臂所包含的6个关节角度记为θ1、θ2、θ3、θ4、θ5、θ6,6个关节对应的连杆长度为固定长度，整个机械臂的运动由6个坐标变换构成，第i坐标变换矩阵由对应θi决定，θi表示两相邻连杆绕公共轴线的夹角；
机械臂末端坐标系相对基座坐标系的变换矩阵
Figure BDA0003902453860000061
H＝A1(θ1)…An(θn)
单杆位姿表示：
Figure BDA0003902453860000062
步骤2：使用仿真软件计算机械臂的关节长度、位置设定角度；
建立DH连杆参数表：
Joint(i) a<sub>i-1</sub>(deg) a<sub>i-1</sub>(mm) θ<sub>i</sub>(deg) d<sub>i</sub>(mm)
1 0 0 θ<sub>1</sub>＝θ<sub>1</sub> 162.5
2 90 0 θ<sub>2</sub>＝θ<sub>2</sub>+180 137.5
3 0 425 θ<sub>3</sub>＝θ<sub>3</sub> 0
4 0 392.2 θ<sub>4</sub>＝θ<sub>4</sub> -4.2
5 -90 0 θ<sub>5</sub>＝θ<sub>5</sub> 99.7
6 90 0 θ<sub>6</sub>＝θ<sub>6</sub>+180 99.6
步骤3：DH推导转换，
Figure BDA0003902453860000071
式中，
Figure BDA0003902453860000072
表示第i-1关节的坐标到第i关节的坐标的齐次变换，
Figure BDA0003902453860000073
表示绕xi-1轴旋转ai-1(deg)角，ai-1(deg)表示两个相邻关节轴的夹角，
Figure BDA0003902453860000074
表示绕Zi轴旋转θi角，
Figure BDA0003902453860000075
表示沿xi-1轴平移距离ai-1(mm)，ai-1(mm)表示两个相邻关节轴公垂线的长度，
Figure BDA0003902453860000076
表示沿Z轴平移距离di,di表示沿两个相邻连杆公共轴线方向的距离,由旋转矩阵和平移向量求出机械臂位姿；
基于
Figure BDA0003902453860000077
Figure BDA0003902453860000078
获取：
Figure BDA0003902453860000079
式中：
r11＝-c6(s1s5+c5(c4(c1s2s3-c1c2c3)+s4(c1c2s3+c1c3s2))-s6(c4(c1c2s3+c1c3s2)-s4(c1s2s3-c1c2c3))
r12＝s6(s1s5+c5(c4(c1s2s3-c1c2c3)+s4(c1c2s3+c1c3s2)))-c6(c4(c1c2s3+c1c3s2)-s4(c1s2s3-c1c2c3))
r13＝c5s1-s5(c4(c1s2s3-c1c2c3)+s4(c1c2s3+c1c3s2))
r21＝c6(c1s5-c5(c4(s1s2s3-c2c3s1)+s4(c2s1s3+c3s1s2)))-s6(c4(c2s1s3+c3s1s2)-s4(s1s2s3-c2c3s1))
r22＝-s6(c1s5-c5(c4(s1s2s3-c2c3s1)+s4(c2s1s3+c3s1s2)))-c6(c4(c2s1s3+c3s1s2)-s4(s1s2s3-c2c3s1))
r23＝-c1c5-s5(c4(s1s2s3-c2c3s1)+s4(c2s1s3+c3s1s2))
r31＝s6(c4(c2c3-s2s3)-s4(c2s3+c2s2))+c5c6(c4(c2s3+c3s2)+s4(c2c3-s2s3))
r32＝c6(c4(c2c3-s2s3)-s4(c2s3+c3s2))-c5s6(c4(c2s3+c3s2)+s4(c2c3-s2s3))
r33＝s5(c4(c2s3+c3s2)+s4(c2c3-s2s3)))
px＝a1+d6(c5s1-s5(c4(c1s1s3-c1c2c3)+s4(c1c2s3+c1c3s2)))-a4(c1s2s3-c1c2c3)-a5(c4(c1s2s3-c1c2c3)+s4(c1c2s3+c1c3s2))-d5(c4(c1s2s3+c1c3s2)-s4(c1s2s3-c1c2c3))+a2c1+d2s1+d3s1+d4s1-a6(s1s5+c5(c4(c1s2s3-c1c2c3)+s4(c1c2s3+c1c3s2)))+a3c1c2
py＝a6(c1s5-c5(c4(s1s2s3-c2c3s1)+s4(c2s1s3+c3s1s2)))-a4(s1s2s3-c2c3s1)-d6(c1c5+s5(c4(s1s2s3c2c3s1)+s4(c1c2s3+c1c3s2)))-a5(c4(s1s2s3-c2c3s1)+s4(c2s1s3+c3s1s2))-d5(c4(c2s1s3+c3s1s2)-s4(s1s2s3-c2c3s1))-d2c1-d3c1-d4c1+a2s1+a3c2s1
pz＝d1+a4(c2s3+c3s2)+a3s2+a5(c4(c2s3+c3s2)+s4(c2c3-s2s3))+d5(c4(c2c3-s2s3)-s4(c2s3+c3s2))+a6c5(c4(c2s3+c2s3)+s4(c2s3-s2s3))+d6s5(c4(c2s3+c3s2)+s4(c2c3-s2s3))
以上简写：si＝sinθi,ci＝cosθi
由于实际机械臂中a的取值只有0，±90，在公式推导中，直接计算出三角函数值带入，不再变化a，以下表达式中的ai表示ai-1,例a3表示连杆参数表中的joint(i＝3)中的a值425；
步骤4：由末端姿态推算出关节角度，根据DH变换，获得
Figure BDA0003902453860000081
其中包含了参数θ1，θ2，θ3，…θn，根据坐标末端执行姿态获得相对于基座坐标系o0x0y0z0的机械臂总的刚体变换矩阵(末端的位置定义了刚体变换矩阵中矩阵中的位移分量，末端的指向向量定义了刚体变换矩阵中的旋转矩阵)记为
Figure BDA0003902453860000082
记
Figure BDA0003902453860000083
R由机械臂末端指向向量生成，p由机械臂末端坐标决定，在逆解问题中，R和P都为已知，另外基座坐标o0x0y0z0与J1坐标系o1x1y1z1定义出
Figure BDA0003902453860000084
将
Figure BDA0003902453860000085
从相对于基座的坐标系变换到相对于J1的坐标系，即
Figure BDA0003902453860000091
由DH变换推导出来的包含参数的θ1，…θn的与根据末端姿态参数获得的
Figure BDA0003902453860000092
完全相等，即：
Figure BDA0003902453860000093
步骤5：角度推导，如果向量
Figure BDA0003902453860000094
在x0y0z0坐标系中的经度记为β，纬度记为α，将
Figure BDA0003902453860000095
变换到
Figure BDA0003902453860000096
根据相似变换，
Figure BDA0003902453860000097
记
Figure BDA0003902453860000098
cβ＝kz，
代入获取
Figure BDA0003902453860000099
vθ＝versθ＝1-cθ，对于给定的旋转
Figure BDA00039024538600000910
Figure BDA00039024538600000911
RK,θ＝R-K,-θ；
Figure BDA00039024538600000912
向量的长度是1，定义一个新的向量rvec＝(θkx,θky,θkz)T，kx、ky、kz表示输入坐标位置，即该向量的方向与
Figure BDA00039024538600000913
一样，角度为θ，由此，向量rvec三个则全部表达了三维坐标系的所有参数，并且，θ不等于0；
步骤6：角度求解；
步骤6.1，求解θ1，θ5，θ6：
基本等式的(2,3)元素相等c1z2-s1z1＝-c5
基本等式的(2,4)元素相等c1p2-p1s1＝-d2-d4-c5d6
整理消去c5,获取s1(p1-z1d6)+c1(z2d6-p2)＝d2+d4
记A＝p1-z1d6；B＝z2d6-p2；r2＝A2+B2
获取θ1＝atan2((d2+d4),
Figure BDA0003902453860000101
将θ1代入s1(p1-z1d6)+c1(z2d6-p2)＝d2+d4
获取
Figure BDA0003902453860000102
基本等式的(2,1)元素相等c1x2-s1x1＝c6s5
基本等式的(2,2)元素相等c1y2-s1y1＝-s5s6
获取θ6＝atan2(±(s1y1-c1y2),±(c1y2-s1x1))；
步骤6.2，求解θ3，θ2：
由
Figure BDA0003902453860000103
等式的(1,4)元素相等。
c1p1+p2s1-d5(c6(c1y1+s1y2)+s6(c1x1+s1x2))-d6(c1z1+s1z2)＝a3c2+a4(c2c3-s2s3)等式的(1,4)元素相等：
p3-d1-d6z3-d5(c6y3+s6x3)＝a3s2+a4(c2s3+c3s2)
X＝c1p1+p2s1-d5(c6(c1y1+s1y2)+s6(c1x1+s1x2))-d6(c1z1+s1z2)
Y＝p3-d1-d6z3-d5(c6y3+s6x3)
推出：
a3c2+a4c3＝X，
a3s2+a4s3＝Y
Figure BDA0003902453860000104
则
Figure BDA0003902453860000105
Figure BDA0003902453860000106
当上式中D的值不介于[-1,1],说明该解无效，需要舍弃；
将θ3带入a3c2+a4c3＝X，a3s2+a4s3＝Y，获取E＝(a3+a4c3)Y-a4s3 X，F＝(a3+a4c3)X+a4s3 Y，获取θ2＝atan2(E,F)；
步骤6.3，求解θ4：
由
Figure BDA0003902453860000111
等式的(1,3)元素相等；
z3(c2c3+c3s2)+c1z1(c2c3-s2s3)+s1z2(c2c3-s2s3)＝c4s5，等式的(2,3)元素相等；
z3(c2c3-s2s3)-c1z1(c2s3+c3s2)-s1z2(c2c3+c3s2)＝s4s5
记：M＝z3(c2c3-s2s3)-c1z1(c2s3+c3s2)-s1z2(c2s3+c3s2)
N＝z3(c2s3+c3s2)+c1z1(c2c3-s2s3)+s1z2(c2c3-s2s3)
获取θ4＝atan2(±M,±N)。
正推过程数据：使用ursim进行仿真数据进行测试
Figure BDA0003902453860000112
Figure BDA0003902453860000121
综上所述：测试误差坐标轴X、Y、Z小于0.01mm,姿态角RX、RY、RZ均小于0.01rad；
逆推过程数据：使用ursim进行仿真数据进行测试
Figure BDA0003902453860000122
综上所述：测试逆推误差角度小于0.01°。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换或改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于刚体变换的DH推导方法，其特征在于，包括以下步骤：
步骤1：将机械臂所包含的6个关节角度记为θ1、θ2、θ3、θ4、θ5、θ6,6个关节对应的连杆长度为固定长度，整个机械臂的运动由6个坐标变换构成，第i坐标变换矩阵由对应θi决定，θi表示两相邻连杆绕公共轴线的夹角；
步骤2：使用仿真软件计算机械臂的关节长度、位置设定角度；建立DH连杆参数表；
步骤3：DH推导转换，
Figure FDA0003902453850000011
式中，
Figure FDA0003902453850000012
表示第i-1关节的坐标到第i关节的坐标的齐次变换，
Figure FDA0003902453850000013
表示绕xi-1轴旋转ai-1(deg)角，ai-1(deg)表示两个相邻关节轴的夹角，
Figure FDA0003902453850000014
表示绕Zi轴旋转θi角，
Figure FDA0003902453850000015
表示沿xi-1轴平移距离ai-1(mm)，ai-1(mm)表示两个相邻关节轴公垂线的长度，
Figure FDA0003902453850000016
表示沿Z轴平移距离di,di表示沿两个相邻连杆公共轴线方向的距离,由旋转矩阵和平移向量求出机械臂位姿；
步骤4：由末端姿态推算出关节角度，根据DH变换，获得
Figure FDA0003902453850000017
其中包含了参数θ1，θ2，θ3，…θn，根据坐标末端执行姿态获得相对于基座坐标系o0x0y0z0的机械臂总的刚体变换矩阵记为
Figure FDA0003902453850000018
记
Figure FDA0003902453850000019
R由机械臂末端指向向量生成，p由机械臂末端坐标决定，在逆解问题中，R和P都为已知，另外基座坐标o0x0y0z0与J1坐标系o1x1y1z1定义出
Figure FDA00039024538500000110
Figure FDA00039024538500000111
将
Figure FDA00039024538500000112
从相对于基座的坐标系变换到相对于J1的坐标系，即
Figure FDA00039024538500000113
由DH变换推导出来的包含参数的θ1，…θn的与根据末端姿态参数获得的
Figure FDA00039024538500000114
完全相等，即：
Figure FDA00039024538500000115
步骤5：角度推导，如果向量
Figure FDA00039024538500000116
在x0y0z0坐标系中的经度记为β，纬度记为α，将
Figure FDA0003902453850000021
变换到
Figure FDA0003902453850000022
Figure FDA0003902453850000023
根据相似变换，
Figure FDA0003902453850000024
记
Figure FDA0003902453850000025
cβ＝kz，
代入获取
Figure FDA00039024538500000212
vθ＝versθ＝1-cθ，
对于给定的旋转
Figure FDA0003902453850000026
Figure FDA0003902453850000027
RK,θ＝R-K,-θ；
步骤6：角度求解。
2.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤1中，机械臂末端坐标系相对基座坐标系的变换矩阵
Figure FDA0003902453850000028
H＝A1(θ1)...An(θn)，单杆位姿表示：
Figure FDA0003902453850000029
3.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤三中，基于
Figure FDA00039024538500000210
Figure FDA00039024538500000211
获取：
Figure FDA0003902453850000031
4.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤四，末端的位置定义了刚体变换矩阵中矩阵中的位移分量，末端的指向向量定义了刚体变换矩阵中的旋转矩阵。
5.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤5中，向量
Figure FDA0003902453850000034
的长度是1，定义一个新的向量rvec＝(θkx,θky,θkz)T,即该向量的方向与
Figure FDA0003902453850000035
一样，角度为θ，向量rvec三个则全部表达了三维坐标系的所有参数，并且，所述θ不等于0。
6.根据权利要求1所述一种基于刚体变换的DH推导方法，其特征在于，所述步骤6包括以下步骤：
步骤6.1，求解θ1，θ5，θ6：
基本等式的(2,3)元素相等c1z2-s1z1＝-c5
基本等式的(2,4)元素相等c1p2-p1s1＝-d2-d4-c5d6
整理消去c5,获取s1(p1-z1d6)+c1(z2d6-p2)＝d2+d4
记A＝p1-z1d6；B＝z2d6-p2；r2＝A2+B2
获取
Figure FDA0003902453850000032
将θ1代入s1(p1-z1d6)+c1(z2d6-p2)＝d2+d4
获取
Figure FDA0003902453850000033
基本等式的(2,1)元素相等c1x2-s1x1＝c6s5
基本等式的(2,2)元素相等c1y2-s1y1＝-s5s6
获取θ6＝atan2(±(s1y1-c1y2),±(c1y2-s1x1))；
步骤6.2，求解θ3，θ2：
由
Figure FDA0003902453850000041
等式的(1,4)元素相等。
c1p1+p2s1-d5(c6(c1y1+s1y2)+s6(c1x1+s1x2))-d6(c1z1+s1z2)＝a3c2+a4(c2c3-s2s3)等
式的(1,4)元素相等：
p3-d1-d6z3-d5(c6y3+s6x3)＝a3s2+a4(c2s3+c3s2)
X＝c1p1+p2s1-d5(c6(c1y1+s1y2)+s6(c1x1+s1x2))-d6(c1z1+s1z2)
Y＝p3-d1-d6z3-d5(c6y3+s6x3)
推出：
a3c2+a4c3＝X，
a3s2+a4s3＝Y
Figure FDA0003902453850000042
则
Figure FDA0003902453850000043
Figure FDA0003902453850000044
当上式中D的值不介于[-1,1],说明该解无效，需要舍弃；
将θ3带入a3c2+a4c3＝X，a3s2+a4s3＝Y，获取E＝(a3+a4c3)Y-a4s3X，F＝(a3+a4c3)X+a4s3Y，获取θ2＝atan2(E,F)；
步骤6.3，求解θ4：
由
Figure FDA0003902453850000045
等式的(1,3)元素相等；
z3(c2c3+c3s2)+c1z1(c2c3-s2s3)+s1z2(c2c3-s2s3)＝c4s5，等式的(2,3)元素相等；
z3(c2c3-s2s3)-c1z1(c2s3+c3s2)-s1z2(c2c3+c3s2)＝s4s5
记：M＝z3(c2c3-s2s3)-c1z1(c2s3+c3s2)-s1z2(c2s3+c3s2)
N＝z3(c2s3+c3s2)+c1z1(c2c3-s2s3)+s1z2(c2c3-s2s3)
获取θ4＝atan2(±M,±N)。
CN202211294024.3A 2022-10-21 2022-10-21 一种基于刚体变换的dh推导方法 Active CN115741672B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211294024.3A CN115741672B (zh) 2022-10-21 2022-10-21 一种基于刚体变换的dh推导方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211294024.3A CN115741672B (zh) 2022-10-21 2022-10-21 一种基于刚体变换的dh推导方法

Publications (2)

Publication Number Publication Date
CN115741672A true CN115741672A (zh) 2023-03-07
CN115741672B CN115741672B (zh) 2024-04-19

Family

ID=85352583

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211294024.3A Active CN115741672B (zh) 2022-10-21 2022-10-21 一种基于刚体变换的dh推导方法

Country Status (1)

Country Link
CN (1) CN115741672B (zh)

Citations (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4823279A (en) * 1986-02-27 1989-04-18 Unimation, Inc. Coordinate conversion system and method for controlling an industrial robot
US5867631A (en) * 1995-08-10 1999-02-02 Fujitsu Limited Manipulator simulation method and apparatus
JP2002301672A (ja) * 2001-04-03 2002-10-15 Yaskawa Electric Corp 産業用ロボットの位置制御方法
CN102243620A (zh) * 2011-06-02 2011-11-16 安凯 六关节机械臂逆运动学问题的快速求解方法
JP2016101632A (ja) * 2014-11-28 2016-06-02 ライフロボティクス株式会社 ロボット装置、ロボット制御装置、ロボット制御方法及びロボット手先効果器
CN106095388A (zh) * 2016-06-12 2016-11-09 任元 一种基于三元角两位置的坐标旋转变换方法
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN107685330A (zh) * 2017-10-18 2018-02-13 佛山华数机器人有限公司 一种六自由度手腕偏置串联机器人的运动学逆解求解方法
CN107756400A (zh) * 2017-10-13 2018-03-06 北京工业大学 一种基于旋量理论的6r机器人逆运动学几何求解方法
CN108121833A (zh) * 2016-11-29 2018-06-05 沈阳新松机器人自动化股份有限公司 一种冗余自由度机器人逆解方法
CN108673505A (zh) * 2018-05-28 2018-10-19 南昌大学 一种机械臂末端精确运动控制方法
WO2018205707A1 (zh) * 2017-05-09 2018-11-15 中国科学院计算技术研究所 机器人的逆运动学求解系统
CN109895101A (zh) * 2019-04-09 2019-06-18 大连理工大学 一种关节型机械臂逆运动学数值唯一解求取方法
DE102018210864B3 (de) * 2018-07-02 2019-07-18 Kuka Deutschland Gmbh Verfahren und System zum Regeln eines Roboters
CN110161850A (zh) * 2019-04-24 2019-08-23 南京航空航天大学 一种工业机器人变参数刚度辨识与建模方法
EP3542973A1 (en) * 2016-11-17 2019-09-25 Fuji Corporation Work robot and work position correction method
WO2019196229A1 (zh) * 2018-04-12 2019-10-17 山东科技大学 一种任意三关节的逆运动学求解方法
CN112084576A (zh) * 2020-08-17 2020-12-15 盐城工学院 车载机械臂运动学建模方法
CN112091979A (zh) * 2020-10-10 2020-12-18 哈尔滨工业大学 一种基于位置级逆运动学的七自由度机械臂限位优化方法
CN112597437A (zh) * 2020-12-29 2021-04-02 深圳市优必选科技股份有限公司 机械臂的逆运动学解析方法、装置及设备
CN112613196A (zh) * 2020-12-30 2021-04-06 配天机器人技术有限公司 一种五轴机器人姿态逆解方法及相关设备
CN112873199A (zh) * 2021-01-08 2021-06-01 西北工业大学 基于运动学与空间插值的机器人绝对定位精度标定方法
KR102342243B1 (ko) * 2020-08-25 2021-12-21 한국로봇융합연구원 로봇암 파라미터 연산 시스템 및 그 방법
CN113858205A (zh) * 2021-10-25 2021-12-31 东南大学 一种基于改进rrt*的七轴冗余机械臂避障算法
CN114995784A (zh) * 2022-07-08 2022-09-02 南昌大学 一种基于dmspso-cil算法的七自由度机械臂解析解优化方法

Patent Citations (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4823279A (en) * 1986-02-27 1989-04-18 Unimation, Inc. Coordinate conversion system and method for controlling an industrial robot
US5867631A (en) * 1995-08-10 1999-02-02 Fujitsu Limited Manipulator simulation method and apparatus
JP2002301672A (ja) * 2001-04-03 2002-10-15 Yaskawa Electric Corp 産業用ロボットの位置制御方法
CN102243620A (zh) * 2011-06-02 2011-11-16 安凯 六关节机械臂逆运动学问题的快速求解方法
JP2016101632A (ja) * 2014-11-28 2016-06-02 ライフロボティクス株式会社 ロボット装置、ロボット制御装置、ロボット制御方法及びロボット手先効果器
CN106095388A (zh) * 2016-06-12 2016-11-09 任元 一种基于三元角两位置的坐标旋转变换方法
EP3542973A1 (en) * 2016-11-17 2019-09-25 Fuji Corporation Work robot and work position correction method
CN108121833A (zh) * 2016-11-29 2018-06-05 沈阳新松机器人自动化股份有限公司 一种冗余自由度机器人逆解方法
WO2018205707A1 (zh) * 2017-05-09 2018-11-15 中国科学院计算技术研究所 机器人的逆运动学求解系统
CN107589934A (zh) * 2017-07-24 2018-01-16 大连理工大学 一种关节型机械臂逆运动学解析解的求取方法
CN107756400A (zh) * 2017-10-13 2018-03-06 北京工业大学 一种基于旋量理论的6r机器人逆运动学几何求解方法
US20190111562A1 (en) * 2017-10-18 2019-04-18 Foshan Huashu Robotics Co., Ltd. Numerical method for obtaining the inverse kinematics of six-degree-of-freedom serial robot with an offset wrist
CN107685330A (zh) * 2017-10-18 2018-02-13 佛山华数机器人有限公司 一种六自由度手腕偏置串联机器人的运动学逆解求解方法
WO2019196229A1 (zh) * 2018-04-12 2019-10-17 山东科技大学 一种任意三关节的逆运动学求解方法
CN108673505A (zh) * 2018-05-28 2018-10-19 南昌大学 一种机械臂末端精确运动控制方法
DE102018210864B3 (de) * 2018-07-02 2019-07-18 Kuka Deutschland Gmbh Verfahren und System zum Regeln eines Roboters
CN109895101A (zh) * 2019-04-09 2019-06-18 大连理工大学 一种关节型机械臂逆运动学数值唯一解求取方法
US20210347045A1 (en) * 2019-04-24 2021-11-11 Nanjing University Of Aeronautics And Astronautics Variable-parameter stiffness identification and modeling method for industrial robot
CN110161850A (zh) * 2019-04-24 2019-08-23 南京航空航天大学 一种工业机器人变参数刚度辨识与建模方法
CN112084576A (zh) * 2020-08-17 2020-12-15 盐城工学院 车载机械臂运动学建模方法
KR102342243B1 (ko) * 2020-08-25 2021-12-21 한국로봇융합연구원 로봇암 파라미터 연산 시스템 및 그 방법
CN112091979A (zh) * 2020-10-10 2020-12-18 哈尔滨工业大学 一种基于位置级逆运动学的七自由度机械臂限位优化方法
CN112597437A (zh) * 2020-12-29 2021-04-02 深圳市优必选科技股份有限公司 机械臂的逆运动学解析方法、装置及设备
CN112613196A (zh) * 2020-12-30 2021-04-06 配天机器人技术有限公司 一种五轴机器人姿态逆解方法及相关设备
CN112873199A (zh) * 2021-01-08 2021-06-01 西北工业大学 基于运动学与空间插值的机器人绝对定位精度标定方法
CN113858205A (zh) * 2021-10-25 2021-12-31 东南大学 一种基于改进rrt*的七轴冗余机械臂避障算法
CN114995784A (zh) * 2022-07-08 2022-09-02 南昌大学 一种基于dmspso-cil算法的七自由度机械臂解析解优化方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
张丰: "空间机械臂装配建模与视觉导引问题研究", 《中国优秀硕士学位论文全文数据库》, 15 February 2002 (2002-02-15) *
张思达: "基于ROS系统的机械臂轨迹规划与抓取", 《中国优秀硕士学位论文全文数据库》, 15 January 2022 (2022-01-15) *

Also Published As

Publication number Publication date
CN115741672B (zh) 2024-04-19

Similar Documents

Publication Publication Date Title
CN111360827B (zh) 一种视觉伺服切换控制方法及系统
CN111230880B (zh) 一种离线编程中的复杂曲面加工轨迹生成方法
CN109976259B (zh) 一种基于vtk的机器人自由曲面工件打磨离线编程方法
CN110039538B (zh) 一种基于大型复杂构件点云信息的机器人轨迹规划方法
CN108324373B (zh) 一种基于电磁定位系统的穿刺手术机器人精确定位实现方法
CN111702762A (zh) 一种工业机器人作业姿态优化方法
CN105014677A (zh) 基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置及方法
CN111168719B (zh) 一种基于定位工装的机器人校准方法及系统
CN111660307A (zh) 一种机器人操作高辅精度的虚拟夹具控制方法及系统
JP3349652B2 (ja) オフラインティーチング方法
CN107102617A (zh) 一种高精度空间椭圆曲线实时插补方法
CN110181522B (zh) 一种五自由度首末对称机械臂逆运动学计算的优化方法
Wiese et al. Kinematic modeling of a soft pneumatic actuator using cubic hermite splines
CN111709095B (zh) 一种面向复杂曲面6d虚拟夹具构造方法
CN112894814B (zh) 一种基于最小二乘法的机械臂dh参数辨识方法
CN104729455A (zh) 一种基于测量数据的管路柔性装焊机器人位姿计算方法
CN115741672A (zh) 一种基于刚体变换的dh推导方法
CN114654466B (zh) 自动标定方法、装置、系统、电子设备及存储介质
CN112276959B (zh) 几何参数与关节零位自标定方法、装置、电子设备及介质
CN113608496B (zh) 空间路径g2转接光顺方法、设备及计算机可读存储介质
CN110555240B (zh) 一种从机器人装配体模型到仿真模型的自动生成方法
JP3380028B2 (ja) 力制御ロボットによるパイプ表面の倣い制御方法
CN111347678B (zh) 一种多关节工业机器人3d打印路径优化方法
CN115570562B (zh) 机器人装配位姿确定方法、装置、机器人及存储介质
CN115533896B (zh) 一种递推式的机器人双臂协同作业路径规划方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant