(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111431329.X (22)申请日 2021.11.29 (71)申请人 山东交通学院 地址 250357 山东省济南市长清区长清大 学科技园海棠路5 001号 (72)发明人 王海燕　阮久宏　杨福广　国兴玉　 杨云　邱健　 (74)专利代理 机构 济南领升专利代理事务所 (普通合伙) 37246 代理人 崔苗苗 (51)Int.Cl. G06F 30/20(2020.01) (54)发明名称 含有被动关节的管片拼装机运动学建模方 法 (57)摘要 本发明公开了一种含有被动关节的管片拼 装机运动学建模方法， 包括： 步骤S1,分析管片拼 装机的运动机理； 步骤S2， 避开含有被动关节的 连杆， 建立含有被动关节的管片拼装机的连杆坐 标系； 步骤S3， 利用平移变换矩阵Trans(px,py, pz)和旋转矩阵Rot(k,θ)， 描述坐标系Oi+1Xi+ 1Yi+1Zi+1(i＝1,2, …,6)相对于OiXiYiZi的位姿变 换， 建立管片拼装机正向运动学模型； 步骤S4， 根 据拼装机正向运动学模型， 考虑管片拼装机关节 运动范围， 建立管片拼装机逆运动学方程； 步骤 S5， 建立两个提升液压缸的位移差ΔL与拼装头 俯仰关节转动量θ4的关系表达式； 步骤S6， 建立 拼装头第一驱动液压缸伸长量ΔL5与拼装头偏 转角θ5的对应关系； 步骤S7， 建立拼装头第二驱 动液压缸伸长量ΔL6与拼装头横滚角θ6的对应 关系。 权利要求书4页 说明书10页 附图3页 CN 114117796 A 2022.03.01 CN 114117796 A 1.一种含有被动关节的管片拼装机运动学建模方法， 其特 征是， 包括以下步骤： 步骤S1,分析 管片拼装机的运动机理； 步骤S2， 避开含有被动关节的连 杆， 建立含有被动关节的管片拼装机的连 杆坐标系； 步骤S3， 利用平移变换矩阵Trans(px,py,pz)和旋转矩阵Rot(k, θ )， 描述坐标系Oi+1Xi+ 1Yi+1Zi+1相对于OiXiYiZi的位姿变换， i 为0～6的整数， 建立管片拼装机正向运动学模型； 步骤S4， 根据拼装机正向运动学模型， 考虑管片拼装机关节运动范围， 建立管片拼装机 逆运动学 方程； 步骤S5， 建立两个提升液压缸的位移差 ΔL与拼装头俯仰关节转动量θ4的关系表达式； 步骤S6， 建立 拼装头第一驱动液压缸伸长量ΔL5与拼装头偏转角 θ5的对应关系； 步骤S7， 建立 拼装头第二驱动液压缸伸长量ΔL6与拼装头横滚角 θ6的对应关系。 2.如权利要求1所述的含有被动关节的管片拼装机运动学建模方法， 其特征是， 所述步 骤S1中的管片拼装机的运动机理包括： 两个平移液压缸推动移动架沿托梁平移， 设其位移 为d1， 回转支架在两个回转马达的作用下相对于移动架转动， 设转动 量为θ2， 两个提升液压 缸固定在回转支架上， 分别与长度固定连杆和含有被动关节的连杆铰接， 举重钳的两端分 别与长度固定连杆和含有被动关节的连杆固连， 将两个提升液压缸 的位移进行比较， 其较 小值为拼装头 的伸缩量， 设伸缩位移为d3， 两提升液压缸的位移差导致拼装头 的俯仰运动 设转动量为θ4， 拼装头第一驱动液压缸驱动拼装 头实现偏转运动设转动量为θ5， 拼装头第二 驱动液压缸驱动拼装头实现横滚运动设转动量 为θ6。 3.如权利要求2所述的含有被动关节的管片拼装机运动学建模方法， 其特征是， 所述步 骤S2中， 将基座标O0X0Y0Z0固定在托梁上， O1X1Y1Z1固连在移动架上， Z1与Z0同向为移动架的 平移方向， O2X2Y2Z2固连在旋 转支架上， Z2为回转支架的回转轴， O1与O2重合， O1与基座标原点 O0沿Z0方向的距离为d1， O3X3Y3Z3固定在提升液压缸上， O3与O2沿X2方向的距离为L2， O4X4Y4Z4 固定在长度固定连杆上， O4是长度固定连杆与提升液压缸的铰接中心， O4与O3沿Z3方向的距 离为d3， O5X5Y5Z5为拼装头回转运动坐标系， Z5为回转轴， 拼装头第一驱动液压缸驱动拼装头 绕Z5实现回转运动， O5与O4沿X4方向的距离为L4， L2与L4大小相等， O6X6Y6Z6为拼装头横滚运动 坐标系， Z6为横滚轴， 拼装 头第二驱动液压缸驱动拼装头绕Z6实现横滚运动， O6与O5沿Z5方向 的距离为d5， O7X7Y7Z7为拼装头的末端坐标系， O7与O6沿X6方向的距离为L6； 将长度固定连杆 与提升液压缸之间的角位移θ4视为可控量， 从而避开含有被动关节的连杆， 建立管片拼装 机的连杆坐标系。 4.如权利要求1所述的含有被动关节的管片拼装机运动学建模方法， 其特征是， 所述步 骤S3中， 平 移变换矩阵Trans(px,py,pz)和旋转矩阵Rot(k, θ )为: 其中， 权　利　要　求　书 1/4 页 2 CN 114117796 A 25.如权利要求3所述的含有被动关节的管片拼装机运动学建模方法， 其特征是， 所述步 骤S3中， 管片拼装机相邻两个连 杆之间的坐标系位姿变换矩阵为： A10＝Trans(0,0,d1) A21＝Rot(z2, π )·Rot(z2, θ2) 其中， 根据相邻连杆之间的坐标变换表达式， 得到拼装头的坐标系相对于基座标的变 换矩阵， 即管片拼装机正向运动学模型： 其中： nx＝cos( θ2+θ4)·cosθ5 ny＝sin( θ2+θ4)·cosθ5 nz＝sinθ5 ox＝‑sin( θ2+θ4)·cosθ6‑cos( θ2+θ4)·sinθ6·sinθ5 oy＝cos( θ2+θ4)·cosθ6‑sin( θ2+θ4)·sinθ6·sinθ5权　利　要　求　书 2/4 页 3 CN 114117796 A 3