说明书一种基于测量数据的管路柔性装焊机器人位姿计算方法
技术领域
本发明涉及一种基于测量数据的管路柔性装焊机器人位姿计算方法，是基于装焊系统的测量数 据及设备位置，计算导管装配时机器人位姿的方法。属于计算机辅助设计制造领域。
背景技术
管路系统在几乎所有机电产品中都有广泛的应用，其功能主要是输送介质的通道。特别是在航 空航天领域，导管都是飞机或者各类航天器的重要组成部分，管路系统能够正常运行是决定产品性能和质 量的一个很重要的条件。
目前，航空航天类管路类型繁多、接口复杂而且多以单件或小批量的生产形式存在。传统的导 管焊接需要设计专用的组合夹具，效率很低，且夹具通用性低，准备周期长。针对不同的焊接导管，需要 设计大量不同的专用夹具，这大大增加了航空航天产品的研制时间和成本，同时也给生产管理带来极大不 便。而且在生产过程中，由于受到操作者经验水平、工装、定位精度以及测量精度等因素的影响，需对导 管焊接后存在较大的误差和变形，无法达到精度要求，可见传统的导管生产工艺已经无法满足现代产品的 制造需求，所以很有必要研究建立面向管路生产的柔性装焊系统。
所设计的管路柔性装焊方案基于视觉测量和机器人，根据测量到的导管数据，通过计算得到机 器人末端执行器位姿，然后控制机器人抓取导管完成对接装配任务。所以如何计算机器人手部在空间的位 姿是实现管路柔性装焊的关键问题。
发明内容
本发明的目的在于提供一种针对基于视觉测量和机器人的管路柔性装焊方案的机器人位姿计 算方法，用以快速计算机器人末端执行器位姿，控制机器人抓取导管运动，实现管路的柔性装焊。
管路柔性装焊方案：管路柔性装焊示意图如图1所示。系统工作时，机器人抓取两段导管在计 算得到的机器人控制文件控制下运动，实现两段导管的对接装配。
图1中出现参数，说明如下：
O-XYZ：全局坐标系取机器人的基座标系。
OT-XTYTZT：机器人工具坐标系即TCP坐标系。
OL-XLYLZL：导管固联坐标系，在导管模型上定义。
导管设计数据，即导管三维数模信息，包括导管对接端面中心点坐标Pc(xc,yc,zc)，导管非对接 端面中心点坐标Pfc(xfc,yfc,zfc)，机器人抓取直线段方向Vl(vxl,vyl,vzl)，导管对接端面法向Vp(vxp,vyp,vzp)。
其他技术参数，说明如下：
导管测量数据：包括导管对接端(余量端)中心点测量坐标Pm(xm,ym,zm)；导管非对接端中心 点测量坐标PFm(xfm,yfm,zfm)；机器人抓取导管测量时的TCP测量位姿TmTCP；机器人回归零位时的TCP零 位位姿TsTCP；导管余量A(Allowance)；导管半径R。
机器人回归零位时，导管对接端(余量端)中心点坐标Ps(xs,ys,zs)，导管非对接端中心点坐标 PFs(xfs,yfs,zfs)，导管对接端(余量端)中心点在机器人TCP坐标系下的坐标Ptcp(xt,yt,zt)，导管非对接端中心 点在机器人TCP坐标系下的坐标PFtcp(xft,yft,zft)，导管位姿TsL，机器人TCP点和导管非对接端中心点在导 管固联坐标系下的坐标Pptcp(xtp,ytp,ztp)和PFp(xfp,yfp,zfp)。
机器人抓取导管对接时，导管对接端中心点坐标即导管对接位置Pd(xd,yd,zd)，导管非对接端中 心点坐标PFj(xfj,yfj,zfj)，导管位姿TjL，机器人TCP对接位姿TjTCP。
本发明提出的一种基于测量数据的管路柔性装焊机器人位姿计算方法，包括下述主要步骤：
步骤一：计算机器人回归零位时导管两端中心点坐标。
步骤二：确定导管固联坐标系，计算机器人回归零位时导管位姿。
步骤三：计算导管对接装配时，机器人TCP位姿。
步骤四：计算机器人TCP坐标系绕全局坐标系三个坐标轴的旋转角度。
本发明给出的一种基于测量数据的管路柔性装焊机器人位姿计算方法，其优点及功效在于：与 传统的管路装配焊接方法相比，本发明省去了繁多的样管和夹具，提升了通用性，降低了成本；能够快速 测量导管参数并且根据测量数据快速计算机器人位姿，控制机器人运动实现导管对接装配，提升了管路生 产效率。
附图说明
图1为本发明的管路柔性装焊示意图。
图2为本发明中机器人位姿计算流程图。
图3为本发明中导管固联坐标系建立示意图。
具体实施方式
下面结合附图，对本发明的技术方案做进一步的说明：
如图2所示，本发明提出一种基于测量数据的管路柔性装焊机器人位姿计算方法，具体步骤如 下：
步骤一：计算机器人回归零位时导管两端中心点坐标。
根据公式(1)，可求得机器人回归零位时，导管两端面中心点坐标Ps(xs,ys,zs)，PFs(xfs,yfs,zfs)，以 及两中心点在机器人TCP坐标系下的坐标Ptcp(xt,yt,zt)，PFtcp(xft,yft,zft)。
P m = T m TCP × P tcp P s = T s TCP × P tcp P Fm = T m TCP × P Ftcp P Fs = T s TCP × P Ftcp - - - ( 1 ) ]]>
步骤二：确定导管固联坐标系，计算机器人零位时导管位姿。
如图3所示，将机器人TCP坐标系相对自身坐标系进行一定的平移和旋转变换即可得到导管 的固联坐标系OL-XLYLZL，导管坐标系原点(OL)取导管对接端面中心点。
求解导管对接端面相对夹持直线段偏角θ：
cos θ = vx 1 · vx p + vy 1 · vy p + vz 1 · vz p vx l 2 + vy l 2 + vz l 2 · vx p 2 + vy p 2 + vz p 2 - - - ( 2 ) ]]>
求解机器人TCP坐标系到导管固联坐标系的转换矩阵T1：
T 1 = 1 0 0 x t 0 cos θ - sin θ y t 0 sin θ cos θ z t 0 0 0 1 - - - ( 3 ) ]]>
可得机器人回归零位时导管位姿：
T s L = T s TCP × T 1 = nx ls ox ls ax ls x s ny ls oy ls ay ls y s nz ls oz ls az ls z s 0 0 0 1 - - - ( 4 ) ]]>
机器人TCP点在导管固联坐标系中的坐标Pptcp(xtp,ytp,ztp)与导管非对接端中心点在导管坐标系 中的坐标PFp(xfp,yfp,zfp)可由下式求解。
P p tcp = T 1 - 1 × 0 0 0 1 - - - ( 5 ) ]]>
PFp＝(TsL)-1×PFs (6)
步骤三：计算导管对接装配时机器人TCP位姿。
导管装配第一步是对接两段导管，即将两端导管对接端面重合。给定导管对接位置Pd(xd,yd,zd)， 其中A段导管保持姿态不变，根据图3所示的导管固联坐标系，将B段导管进行平移和旋转操作，使其位 姿与A段导管位姿相同即可保证A、B段导管端面重合。
求解导管的对接位姿：
T e L = nx ls ox ls ax ls x d ny ls oy ls ay ls y d nz ls oz ls az ls z d 0 0 0 1 - - - ( 7 ) ]]>
求解导管固联坐标系到机器人TCP坐标系的转换矩阵T2：
T 2 = 1 0 0 x tp 0 cos ( - θ ) - sin ( - θ ) y tp 0 sin ( - θ ) cos ( - θ ) z tp 0 0 0 1 - - - ( 8 ) ]]>
导管对接时机器人TCP位姿TeTCP为：
TeTCP＝TeL×T2 (9)
接下来第二步是将导管绕对接端面法向即导管坐标系Y轴进行旋转，以保证导管非对接端面的 相对位姿。
计算导管非对接端中心点的理论坐标PFe(xfe,yfe,zfe)：
x fe = x d + ( x fc - x c ) y fe = y d + ( y fc - y c ) z fe = z d + ( z fc - z c ) - - - ( 10 ) ]]>
假设旋转角度为Φ，则导管坐标系的旋转矩阵为T3：
T 3 = cos φ 0 sin φ 0 0 1 0 0 - sin φ 0 cos φ 0 0 0 0 1 - - - ( 11 ) ]]>
求解导管经旋转变换后的位姿TjL：
TjL＝TeL×T3 (12)
导管经旋转变换后，非对接端中心点坐标PFj(xfj,yfj,zfj)为：
PFj＝TjL×PFp (13)
根据下式求解旋转角度Φ：
PFj＝PFe (14)
求解得到机器人最终TCP位姿TjTCP：
T j TCP = T j L × T 2 = nx ox ax a ny oy ay b nz oz az c 0 0 0 1 - - - ( 14 ) ]]>
步骤四：计算机器人TCP坐标系绕全局坐标系三个坐标轴的旋转角度。
确定机器人TCP坐标系绕全局坐标系各轴的旋转顺序，选定旋转顺序为Z→Y→X，设各旋转 角度依次为α、β、γ。
则根据下式可求得机器人TCP位姿旋转角。
T j TCP = cos α cos β - sin α cos γ + cos α sin β sin γ sin α sin γ + cos α sin β cos γ 0 sin α cos β cos α cos γ + sin α sin β sin γ - cos α sin γ + sin α sin β cos γ 0 - sin β cos β sin γ cos β cos γ 0 0 0 0 1 = nx ox ax a ny oy ay b nz oz az c 0 0 0 1 - - - ( 15 ) ]]>
下表所示为管路柔性装焊系统的测量数据：
下表所示为计算得到的机器人位姿参数：