CN102699733A

CN102699733A - Method and device for measuring movement locus of automatic tool changing mechanical arm

Info

Publication number: CN102699733A
Application number: CN2012101928916A
Authority: CN
Inventors: 卢晓红; 韩鹏卓; 贾振元; 王福吉; 刘巍
Original assignee: Dalian University of Technology
Current assignee: Dalian University of Technology
Priority date: 2012-06-12
Filing date: 2012-06-12
Publication date: 2012-10-03
Anticipated expiration: 2032-06-12
Also published as: CN102699733B

Abstract

The invention discloses a method and a device for measuring the motion trajectory of an automatic tool changing mechanical arm, belonging to the field of tracking and measuring the spatial motion trajectory of a mechanical arm of an automatic tool changing system in a machining center. This method is a non-contact three-dimensional online measurement method based on two-dimensional PSD. The light source of the measured target is installed on the ATC mechanical arm to be measured, and the light source is projected on the PSD camera composed of two position-sensitive detectors through the lens. A light spot is formed on the two-dimensional sensing screen, and the two PSD cameras respectively output two-dimensional coordinate values under the excitation of the light spot, and output the voltage value corresponding to the coordinate position after passing through the signal processor, and the computer collects and saves the corresponding signal through the voltage acquisition card. The method has a fast response speed and meets the requirements of real-time online measurement. Through the motion trajectory of the mechanical arm, the positioning accuracy, motion stability and other motion states during the tool change process can be judged, which provides a quantitative method for the reliability detection of the tool magazine.

Description

Method and device for measuring motion track of automatic tool changing robot arm

技术领域 technical field

本发明属于加工中心自动换刀系统机械手臂的空间运动轨迹跟踪测量领域，是利用二维位置敏感探测器构成的三维测量装置，实现机械手臂运动轨迹的非接触式在线测量。The invention belongs to the field of space motion track tracking measurement of a mechanical arm of an automatic tool change system of a machining center, and is a three-dimensional measuring device composed of a two-dimensional position sensitive detector, which realizes non-contact on-line measurement of the motion track of a mechanical arm.

背景技术 Background technique

近年来，随着装备制造业的快速发展，以加工中心为代表的高档数控机床的需求量与日俱增，作为加工中心的关键功能部件—大型刀库的自动换刀系统(Automatic Tool Changer，简称ATC)的研发受到国内外政府、企业的高度重视。ATC作为加工中心刀库的重要组成部分，是刀库与机床主轴的中间桥梁，主要功能是将加工使用刀具自动装夹到主轴上，并把主轴上不再使用的刀具卸载放回刀库中。ATC的应用极大的提高了机床的加工效率，使一次装夹完成多道工序或全部工序成为可能，已成为现代加工中心必不可少的功能部件，自发明至今已经形成以转塔式和刀库式为主的多种类型的成品。大型刀库式自动换刀系统储刀能力强，适合复杂多工序加工场合，成为目前制造业的研究热点。经过几年的创新、研发和制造，国内外知名机床生产厂商都已形成了具有一定规模的、具有自主知识产权的先进大型刀库自动换刀系统，但是对于自动换刀系统的可靠性检测，从国家标准、机械行业到企业标准中都只是一些总体要求，没有规范的检测方法和可以量化的设备。ATC具有启动频繁、工作动作多、传递机构复杂、运动精度要求高及测量空间狭窄等特点，导致其加工运动轨迹的跟踪测量很困难。对于目前广泛使用的弧面凸轮式ATC来说，运动轨迹的测量结果可以有效的反映出凸轮的制造误差和装配误差对ATC定位精度的影响程度，对刀库自动换刀系统的可靠性检测具有重大意义。目前，国内外对机构运动轨迹的跟踪测量的研究并不多，主要集中在简单机构的平面运动轨迹的测量方面，如秦威的《基于CCD技术的平面机构运动研究》，测量目标处于二维空间内，测量精度要求较低，而ATC机械手臂的运动轨迹是三维空间内的复杂曲线。In recent years, with the rapid development of the equipment manufacturing industry, the demand for high-end CNC machine tools represented by machining centers is increasing day by day. The research and development of the company are highly valued by domestic and foreign governments and enterprises. As an important part of the tool magazine of the machining center, ATC is the bridge between the tool magazine and the machine tool spindle. Its main function is to automatically clamp the tools used for processing to the spindle, and unload the unused tools on the spindle and put them back into the tool magazine. . The application of ATC has greatly improved the processing efficiency of the machine tool, making it possible to complete multiple processes or all processes in one clamping. It has become an indispensable functional part of modern machining centers. Since its invention, it has formed a turret and tool Various types of finished products based on library type. The large-scale tool magazine-type automatic tool change system has a strong tool storage capacity and is suitable for complex multi-process processing occasions, and has become a research hotspot in the manufacturing industry. After several years of innovation, R&D and manufacturing, well-known machine tool manufacturers at home and abroad have formed an advanced large-scale tool magazine automatic tool change system with a certain scale and independent intellectual property rights. However, for the reliability detection of the automatic tool change system, From national standards, machinery industry to enterprise standards, there are only some general requirements, and there are no standardized testing methods and quantifiable equipment. ATC has the characteristics of frequent start-up, many working actions, complex transmission mechanism, high motion precision requirements, and narrow measurement space, which makes it difficult to track and measure its machining motion trajectory. For the arc cam ATC widely used at present, the measurement results of the motion trajectory can effectively reflect the degree of influence of the manufacturing error and assembly error of the cam on the positioning accuracy of the ATC, which is of great significance to the reliability detection of the automatic tool change system of the tool magazine. Great significance. At present, there are not many researches on the tracking and measurement of mechanism motion trajectory at home and abroad, mainly focusing on the measurement of the plane motion trajectory of simple mechanisms, such as Qin Wei's "Research on the Motion of Plane Mechanism Based on CCD Technology", the measurement target is in two dimensions In space, the measurement accuracy is relatively low, and the trajectory of the ATC robot arm is a complex curve in three-dimensional space.

发明内容 Contents of the invention

本发明要解决的技术难题是克服现有技术的缺欠，由于加工中心ATC机构复杂，驱动凸轮加工精度难于检测，装配精度对终端运动误差的影响难于评价，整体结构的封闭性增加了运动状态的检测难度，故形成有效的可靠性检测方法困难。本发明结合位置敏感探测器(PositionSensory Device，简称PSD)和双目视觉三维测量原理发明了一种基于两个二维PSD相机的非接触式三维测量方法和装置。根据所测量的机械手臂的实际运动轨迹来研究自动换刀系统的故障状态，并形成一种有效的刀库换刀机构可靠性检测方法。本发明采用PSD提取目标光点坐标的方式，响应速度快，满足实时在线测量要求；通过机械手臂的运动轨迹可以判断换刀过程中的定位精度、运动平稳性等运动状态，为刀库可靠性检测提供量化方法。The technical problem to be solved by the present invention is to overcome the shortcomings of the prior art. Due to the complex ATC mechanism of the machining center, it is difficult to detect the machining accuracy of the driving cam, and it is difficult to evaluate the influence of assembly accuracy on the terminal motion error. It is difficult to detect, so it is difficult to form an effective reliability detection method. The present invention combines a position sensitive detector (PositionSensory Device, referred to as PSD) and a binocular vision three-dimensional measurement principle to invent a non-contact three-dimensional measurement method and device based on two two-dimensional PSD cameras. According to the measured actual motion trajectory of the mechanical arm, the fault state of the automatic tool change system is studied, and an effective method for detecting the reliability of the tool change mechanism of the tool magazine is formed. The present invention adopts the way of PSD to extract the coordinates of the target light spot, which has a fast response speed and meets the requirements of real-time online measurement; through the motion trajectory of the mechanical arm, the motion states such as positioning accuracy and motion stability during the tool change process can be judged, which is the reliability of the tool magazine. Assays provide quantification methods.

本发明要解决的技术方案是一种自动换刀机械手臂运动轨迹的测量方法，该方法是一种基于二维PSD的非接触式三维在线测量方法。作为被测目标的光源安装在被测量ATC机械手臂上，测量原理如图1所示，光源通过透镜投射在由两个位置敏感探测器组成的PSD相机的二维感应屏上形成光斑，两个PSD相机在光斑的激发下各自输出二维坐标值，经过信号处理器后输出与坐标位置对应的电压值，计算机通过电压采集卡将相应信号采集保存，由两组采集到的二维坐标值反算出被测光源的三维坐标值。最后，依据高频采集的数据计算出一系列离散点构成机械手臂的运动轨迹。The technical solution to be solved by the present invention is a method for measuring the motion track of an automatic tool changing robot arm, which is a non-contact three-dimensional online measurement method based on two-dimensional PSD. The light source as the target to be measured is installed on the ATC robot arm to be measured. The measurement principle is shown in Figure 1. The light source is projected through the lens on the two-dimensional sensing screen of the PSD camera composed of two position-sensitive detectors to form a light spot. The PSD cameras respectively output two-dimensional coordinate values under the excitation of the light spot, and output the voltage value corresponding to the coordinate position after passing through the signal processor. The computer collects and stores the corresponding signal through the voltage acquisition card, and the two-dimensional coordinate values collected by the two groups are reverse Calculate the three-dimensional coordinate value of the measured light source. Finally, a series of discrete points are calculated to form the trajectory of the robotic arm based on the data collected at high frequency.

1）在三维测量之前要获得PSD相机的高精度的内外参数，测量方法如下：1) To obtain high-precision internal and external parameters of the PSD camera before three-dimensional measurement, the measurement method is as follows:

设被测目标光源所在的坐标系为世界坐标系W，投射光斑所在坐标系为PSD坐标系S，镜头坐标系L、图像坐标系I、PSD坐标系S参照图2所示，设世界坐标系W下点P (X_W，Y_W，Z_W)，其对应的投射点P’在PSD坐标系S下的坐标为(u,v)，二者之间的转换关系为：Let the coordinate system where the target light source is located be the world coordinate system W, the coordinate system where the projected spot is located be the PSD coordinate system S, the lens coordinate system L, the image coordinate system I, and the PSD coordinate system S refer to Figure 2, and set the world coordinate system The coordinates of the point P (X _W , Y _W , Z _W ) under W and its corresponding projected point P' in the PSD coordinate system S are (u, v), and the conversion relationship between the two is:

${Z Z}_{L L} [\begin{matrix} μ μ \\ ν ν \\ 11 \end{matrix}] = = [\begin{matrix} {f f}_{x x} & 00 & {μ μ}_{00} & 00 \\ 00 & {f f}_{y the y} & {ν ν}_{00} & 00 \\ 00 & 00 & 11 & 00 \end{matrix}] \cdot \cdot [\begin{matrix} R R & T T \\ 00 & 11 \end{matrix}] \cdot \cdot [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \\ 11 \end{matrix}] = = [\begin{matrix} A A & 00 \end{matrix}] \cdot \cdot [\begin{matrix} R R & T T \\ 00 & 11 \end{matrix}] \cdot &Center Dot; [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \\ 11 \end{matrix}] - - - - - - ((11))$

其中，A为内参数矩阵，其包含了焦距f和偏移量(u₀,v₀)，R、T分别为世界坐标系到镜头坐标系的旋转矩阵和平移矩阵，[R T]为PSD相机的外参数矩阵，Z_L为测量目标在镜头坐标系下的Z轴坐标值；Among them, A is the internal parameter matrix, which includes the focal length f and offset (u ₀ , v ₀ ), R and T are the rotation matrix and translation matrix from the world coordinate system to the lens coordinate system respectively, and [R T] is the PSD camera The external parameter matrix, Z _L is the Z-axis coordinate value of the measurement target in the lens coordinate system;

使用图3所示的平面模板装置来测量PSD相机的内外参数，设世界坐标系W的原点在模板左上角光源的中心，水平方向为X轴，垂直方向为Y轴，垂直于平面向里为Z轴，则Z_w=0：Use the plane template device shown in Figure 3 to measure the internal and external parameters of the PSD camera. Let the origin of the world coordinate system W be at the center of the light source in the upper left corner of the template, the horizontal direction is the X axis, the vertical direction is the Y axis, and the vertical direction is the Y axis. Z axis, then Z _w =0:

${Z Z}_{L L} [\begin{matrix} μ μ \\ ν ν \\ 11 \end{matrix}] = = [\begin{matrix} {f f}_{x x} & 00 & {μ μ}_{00} & 00 \\ 00 & {f f}_{y the y} & {ν ν}_{00} & 00 \\ 00 & 00 & 11 & 00 \end{matrix}] \cdot \cdot [\begin{matrix} {R R}_{11} & {R R}_{22} & T T \\ 00 & 00 & 11 \end{matrix}] \cdot \cdot [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ 11 \end{matrix}] = = H h \cdot \cdot [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ 11 \end{matrix}] - - - - - - ((22))$

根据张氏标定法求解内外参数，固定好PSD相机和模板后启动控制电路38，从光源阵列36左上角起扫描式依次点亮光源，同时采集相应的PSD的输出坐标值，直到最后一个光源测量完毕，将采集到的数据和相应的光源的在世界坐标系下的坐标值代入下述方程： $[\begin{matrix} X_{W} & Y_{W} & 1 & 0 & 0 & 0 & - μ \cdot X_{W} & - μ \cdot Y_{W} & - μ \\ 0 & 0 & 0 & X_{W} & Y_{W} & 1 & - ν \cdot X_{W} & - ν \cdot Y_{W} & - ν \end{matrix}] \cdot [\begin{matrix} h_{11} \\ h_{12} \\ h_{13} \\ h_{21} \\ h_{22} \\ h_{21} \\ h_{31} \\ h_{32} \\ h_{33} \end{matrix}] = [\begin{matrix} 0 \\ 0 \end{matrix}]$ $- - - (3)$ Solve the internal and external parameters according to Zhang’s calibration method, start the control circuit 38 after fixing the PSD camera and the template, light the light sources in a scanning manner from the upper left corner of the light source array 36, and collect the corresponding PSD output coordinate values at the same time, until the last light source measurement After completion, substitute the collected data and the coordinate values of the corresponding light source in the world coordinate system into the following equation: $[\begin{matrix} x_{W} & Y_{W} & 1 & 0 & 0 & 0 & - μ &Center Dot; x_{W} & - μ \cdot Y_{W} & - μ \\ 0 & 0 & 0 & x_{W} & Y_{W} & 1 & - ν \cdot x_{W} & - ν \cdot Y_{W} & - ν \end{matrix}] &Center Dot; [\begin{matrix} h_{11} \\ h_{12} \\ h_{13} \\ h_{twenty one} \\ h_{twenty two} \\ h_{twenty one} \\ h_{31} \\ h_{32} \\ h_{33} \end{matrix}] = [\begin{matrix} 0 \\ 0 \end{matrix}]$ $- - - (3)$

模板中的光源数量足够多时，通过最小二乘法求解出单应性矩阵H，改变相机或者模板的方位，就能求出不同单应性矩阵，将多组H代入下述方程：When the number of light sources in the template is large enough, the homography matrix H is solved by the least square method, and different homography matrices can be obtained by changing the orientation of the camera or the template, and multiple groups of H are substituted into the following equation:

$[\begin{matrix} {V V}_{1212}^{T T} \\ {(({V V}_{1111} - - {V V}_{22 twenty two}))}^{T T} \end{matrix}] \cdot \cdot b b = = 00 - - - - - - ((44))$

其中in

$V_{ij} = {[\begin{matrix} h_{i 1} \cdot h_{j 1} & h_{i 1} \cdot h_{j 2} + h_{i 2} \cdot h_{j 1} & h_{i 1} \cdot h_{j 3} + h_{i 3} \cdot h_{j 1} & h_{i 2} \cdot h_{j 2} & h_{i 2} \cdot h_{j 3} + h_{i 3} \cdot h_{j 2} & h_{i 3} \cdot h_{j 3} \end{matrix}]}^{T}$ ，b是对称矩阵B的六个独立元素构成的向量。 $V_{ij} = {[\begin{matrix} h_{i 1} \cdot h_{j 1} & h_{i 1} \cdot h_{j 2} + h_{i 2} \cdot h_{j 1} & h_{i 1} &Center Dot; h_{j 3} + h_{i 3} \cdot h_{j 1} & h_{i 2} \cdot h_{j 2} & h_{i 2} \cdot h_{j 3} + h_{i 3} &Center Dot; h_{j 2} & h_{i 3} &Center Dot; h_{j 3} \end{matrix}]}^{T}$ , b is a vector of six independent elements of the symmetric matrix B.

$B B = = {(({A A}^{- - 11}))}^{T T} {A A}^{- - 11} = = [\begin{matrix} \frac{11}{{f f}_{x x}^{22}} & 00 & - - \frac{{μ μ}_{00}}{{f f}_{x x}^{22}} \\ 00 & \frac{11}{{f f}_{y the y}^{22}} & - - \frac{{ν ν}_{00}}{{f f}_{y the y}^{22}} \\ - - \frac{{μ μ}_{00}}{{f f}_{x x}^{22}} & - - \frac{{ν ν}_{00}}{{f f}_{y the y}^{22}} & \frac{{μ μ}_{00}^{22}}{{f f}_{x x}^{22}} + + \frac{{ν ν}_{00}^{22}}{{f f}_{y the y}^{22}} + + 11 \end{matrix}]$

当改变的方位次数足够多时，通过最小二乘法求出b，并反算出相机内参数矩阵A，将A代入下式：When the number of orientation changes is sufficient, b is obtained by the least square method, and the camera internal parameter matrix A is reversely calculated, and A is substituted into the following formula:

R₁=A^-1H₁，R₂=A^-1H₂，T=A^-1H₃(5)R ₁ =A ^-1 H ₁ , R ₂ =A ^-1 H ₂ , T=A ^-1 H ₃ (5)

然后通过将R₁和R₂通过正交扩展出R₃ ，求出相机外参数矩阵[R T]。Then R ₃ is obtained by extending R ₁ and R ₂ orthogonally to obtain the camera extrinsic parameter matrix [RT].

2）双PSD相机的三维定位方法如下：2) The three-dimensional positioning method of dual PSD cameras is as follows:

散射目标光源发出的光束经过镜头汇聚后投影在PSD相机感应屏上，世界坐标系下目标光源中心坐标（X_W，Y_W，Z_W）与PSD坐标系下投影光斑中心坐标（μ，ν）的关系如下式：The light beam emitted by the scattered target light source is projected on the PSD camera sensor screen after being converged by the lens. The center coordinates of the target light source in the world coordinate system (X _W , Y _W , Z _W ) and the center coordinates of the projected spot in the PSD coordinate system (μ, ν) The relationship is as follows:

$[\begin{matrix} {m m}_{11} - - {m m}_{99} μ μ & {m m}_{22} - - {m m}_{1010} μ μ & {m m}_{33} - - {m m}_{1111} μ μ \\ {m m}_{55} - - {m m}_{99} ν ν & {m m}_{66} - - {m m}_{1010} ν ν & {m m}_{77} - - {m m}_{1111} ν ν \end{matrix}] \cdot \cdot [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \end{matrix}] = = [\begin{matrix} {m m}_{1212} μ μ - - {m m}_{44} \\ {m m}_{1212} ν ν - - {m m}_{88} \end{matrix}] - - - - - - ((66))$

其中，in,

$M m = = [\begin{matrix} {m m}_{11} & {m m}_{22} & {m m}_{33} & {m m}_{44} \\ {m m}_{55} & {m m}_{66} & {m m}_{77} & {m m}_{88} \\ {m m}_{99} & {m m}_{1010} & {m m}_{1111} & {m m}_{1212} \end{matrix}] = = [\begin{matrix} A A & 00 \end{matrix}] \cdot \cdot [\begin{matrix} R R & T T \\ 00 & 11 \end{matrix}]$

利用单个PSD相机输出的二维坐标可以列出如上式的两个方程，两个PSD相机同时感应同个目标光源，可以列出四个方程并通过最小二乘法求解出目标光源中心在世界坐标系下的坐标值；Using the two-dimensional coordinates output by a single PSD camera, the two equations of the above formula can be listed. Two PSD cameras simultaneously sense the same target light source. Four equations can be listed and the center of the target light source in the world coordinate system can be solved by the least square method. The coordinate value under;

3）机械手臂运动轨迹的测量方法如下：3) The measurement method of the movement trajectory of the robot arm is as follows:

将目标光源安装在加工中心ATC的机械手臂上，其随换刀动作在空间形成机械手臂的运动轨迹，运动轨迹测量采用目标光源的三维坐标值计算和离散点曲线拟合方法。PSD相机的感应面是一个二维平面，投影光斑的二维坐标值与目标光点的三维坐标值通过PSD相机的内外参数矩阵来表示，两个二维PSD相机输出的坐标值可以反解出目标点的三维坐标，一系列离散目标点拟合出加工过程中机械手臂的运动轨迹。The target light source is installed on the mechanical arm of the machining center ATC, which forms the motion trajectory of the mechanical arm in space with the tool change action. The motion trajectory measurement adopts the calculation of the three-dimensional coordinate value of the target light source and the discrete point curve fitting method. The sensing surface of the PSD camera is a two-dimensional plane. The two-dimensional coordinate values of the projected spot and the three-dimensional coordinate values of the target light spot are represented by the internal and external parameter matrix of the PSD camera. The coordinate values output by the two two-dimensional PSD cameras can be reversed. The three-dimensional coordinates of the target point, a series of discrete target points fit the movement trajectory of the robot arm during the processing.

4）发明的测量装置是以两个PSD相机为核心具有多自由度的测量装置。测量装置本身具有竖直位移、水平位移、旋转角度及俯仰角度四个自由度的调整，装置的结构如图6所示：4) The invented measuring device is a measuring device with multiple degrees of freedom based on two PSD cameras. The measuring device itself has the adjustment of four degrees of freedom of vertical displacement, horizontal displacement, rotation angle and pitch angle. The structure of the device is shown in Figure 6:

底座23的立板上加工有螺栓孔，带断电保护的Z轴电控平台28用螺栓固定在立板上。电机17通过滚珠丝杠18带动承载工作台24在竖直方向移动，通过一组螺栓22在滑动承载工作台24上安装两个高度差可调的滑轨21，滑轨的顶部的水平燕尾导轨与小托板20配合，小托板20上的PSD相机在水平方向位置可调。通过串联方式在小托板20上依次装配手动调整旋转平台25和角位移平台26，左PSD相机27和右PSD相机19安装在角位移平台的工作台上。Bolt holes are processed on the vertical plate of the base 23, and the Z-axis electric control platform 28 with power-off protection is fixed on the vertical plate with bolts. The motor 17 drives the bearing table 24 to move in the vertical direction through the ball screw 18, and two slide rails 21 with adjustable height difference are installed on the sliding bearing table 24 through a group of bolts 22. The horizontal dovetail guide rail at the top of the slide rails Cooperating with the small supporting board 20, the position of the PSD camera on the small supporting board 20 can be adjusted in the horizontal direction. Manually adjust the rotating platform 25 and the angular displacement platform 26 sequentially assembled on the small pallet 20 in series, and the left PSD camera 27 and the right PSD camera 19 are installed on the workbench of the angular displacement platform.

5）测量装置的核心PSD相机的装配结构如图7所示：5) The assembly structure of the core PSD camera of the measurement device is shown in Figure 7:

一种自动换刀机械手臂运动轨迹的测量方法，该方法采用的装置是PSD相机整体结构由聚焦部分、探测器、微调部分和连接框架四部分组成；聚焦部分使用的是工业定焦镜头29，调节镜头中的光圈改变PSD感应屏上投影光斑的大小和光度，工业镜头29与L型连接板30通过标准C口连接，位置敏感探测器31采用10针通讯电缆与信号处理器连接，位置敏感探测器31安装在手动微进给平台35的工作台上，通过调整微进给平台35可以调节位置敏感探测器31与工业镜头29之间的距离，L型连接板30和定位转接板34构成了连接框架部分，位置敏感探测器31通过定位销32和位置敏感探测器31底面的螺纹定位并夹紧在定位转接板34，定位转接板34通过内六角螺钉33固定在一维手动平移台35的工作台上。A method for measuring the motion trajectory of an automatic tool changing robot arm. The device used in the method is a PSD camera. The overall structure is composed of four parts: a focusing part, a detector, a fine-tuning part and a connecting frame; the focusing part uses an industrial fixed-focus lens 29, Adjust the aperture in the lens to change the size and luminosity of the projected spot on the PSD sensing screen. The industrial lens 29 is connected to the L-shaped connecting plate 30 through the standard C port. The position sensitive detector 31 is connected to the signal processor with a 10-pin communication cable. The detector 31 is installed on the workbench of the manual micro-feeding platform 35, the distance between the position-sensitive detector 31 and the industrial lens 29 can be adjusted by adjusting the micro-feeding platform 35, the L-shaped connecting plate 30 and the positioning adapter plate 34 The connection frame part is formed, the position sensitive detector 31 is positioned and clamped on the positioning adapter plate 34 through the positioning pin 32 and the thread on the bottom surface of the position sensitive detector 31, and the positioning adapter plate 34 is fixed on the one-dimensional manual by the hexagon socket head screw 33. Translation stage 35 on the workbench.

本发明的有益效果是为加工中心ATC可靠性检测提供一种量化方法和装置。基于ATC传动机构、封闭性强的特点，发明了一种光学非接触式在线运动轨迹测量方法和装置，对加工过程中ATC机械手臂进行运动轨迹实时跟踪记录，并将其加工动作分解，检测运动的定位精度、运动准确性及工作稳定性，从而是ATC可靠性检测形成量化标准。The beneficial effect of the invention is to provide a quantitative method and device for ATC reliability detection of the machining center. Based on the characteristics of ATC transmission mechanism and strong sealing, an optical non-contact online motion trajectory measurement method and device are invented, which can track and record the motion trajectory of the ATC robot arm in real time during the processing process, decompose its processing action, and detect the movement Positioning accuracy, motion accuracy and working stability, thus forming a quantitative standard for ATC reliability testing.

附图说明 Description of drawings

图1为双PSD相机轨迹测量原理示意图。Figure 1 is a schematic diagram of the principle of trajectory measurement with dual PSD cameras.

图2为测量系统坐标系定义图。Figure 2 is a definition diagram of the coordinate system of the measurement system.

图3为单PSD相机内外参数测量示意图。其中，36-光源阵列，37-平面标定模板，38-控制电路。Figure 3 is a schematic diagram of measuring internal and external parameters of a single PSD camera. Among them, 36-light source array, 37-plane calibration template, 38-control circuit.

图4为刀库自动换刀机构结构图。其中：1-机身，2-机械手臂，3-轴套，4-凸轮顶壳，5-弧面凸轮，6-端面滑块，7-周面转块，8-传递轴，9-摆杆，10-传递杆，11-主锥齿轮，12-次锥齿轮，13-回转轴。Figure 4 is a structural diagram of the automatic tool change mechanism of the tool magazine. Among them: 1-body, 2-mechanical arm, 3-shaft sleeve, 4-cam top shell, 5-arc cam, 6-end slider, 7-circumferential turn block, 8-transmission shaft, 9-pendulum Rod, 10-transmission rod, 11-primary bevel gear, 12-secondary bevel gear, 13-revolving shaft.

图5为ATC机械手臂运动轨迹测量示意图。其中：14-1号LED光源，15-2号LED光源，16-双PSD测量装置。Fig. 5 is a schematic diagram of the motion track measurement of the ATC mechanical arm. Among them: No. 14-1 LED light source, No. 15-2 LED light source, 16- double PSD measuring device.

图6为双PSD测量装置详细结构。其中：17-电机，18-丝杠，19-右PSD相机，20-小托板，21-滑轨，22-六角头螺栓，23-底座，24-承载台，25- 旋转平台，26-角位移平台，27-左PSD相机，28-Z轴位移平台。Figure 6 shows the detailed structure of the dual PSD measurement device. Among them: 17-motor, 18-lead screw, 19-right PSD camera, 20-small pallet, 21-slide rail, 22-hex head bolt, 23-base, 24-carrying platform, 25-rotary platform, 26- Angular displacement platform, 27-left PSD camera, 28-Z axis displacement platform.

图7为PSD相机结构图。其中：29-工业镜头，30-L型型连接板，31-位置敏感探测器，32-定位销，33-内六角螺栓，35-一维手动平移台，。Figure 7 is a structural diagram of a PSD camera. Among them: 29-industrial lens, 30-L-type connecting plate, 31-position sensitive detector, 32-locating pin, 33-hexagon socket bolt, 35-one-dimensional manual translation stage,.

图8为三维测量流程图。Fig. 8 is a flowchart of three-dimensional measurement.

具体实施方式 Detailed ways

本发明是用于ATC工作过程中运动轨迹的在线测量，在ATC机械手臂上安装两个LED光源作为跟踪目标，利用两个二维位置敏感探测器检测投影光斑坐标，然后计算出目标点的三维坐标值。这种测量装置对弧面凸轮式ATC机械手臂上的目标点进行测量，得到自动换刀机构的实际运动状态，并与理想运动情况对比，从而对 “掉刀”、“抖动”、“运动不到位”等异常现象做出预测、评估，形成了一种ATC 可靠性检测手段。The invention is used for on-line measurement of the motion trajectory during the ATC working process. Two LED light sources are installed on the ATC mechanical arm as the tracking target, and two two-dimensional position sensitive detectors are used to detect the projection spot coordinates, and then the three-dimensional position of the target point is calculated. coordinate value. This measuring device measures the target point on the arc cam type ATC mechanical arm, obtains the actual motion state of the automatic tool change mechanism, and compares it with the ideal motion state, so as to "drop the tool", "jitter" and "not moving" "In place" and other abnormal phenomena are predicted and evaluated, forming an ATC reliability detection method.

下面结合附图和技术方案详细说明本发明的具体实施：The concrete implementation of the present invention is described in detail below in conjunction with accompanying drawing and technical scheme:

(1) 二维PSD相机整体结构(1) The overall structure of the 2D PSD camera

二维PSD相机的作用是将测量目标光源投射到PSD感应屏形成光斑，并输出光斑中心坐标，其原理与数码相机相似，故以此命名。本发明设计的二维PSD相机的结构如图7所示，在L型连接板30的壁上加工一个通螺纹孔，尺寸规格根据需要的工业镜头接口型号而定，一般为C型、F型或CS型，本设计中采用焦距25mm的定焦C口工业镜头29。日本滨松公司的C10443-03型位置敏感探测器31放置在L型连接板30的后方，保证感应屏位于镜头后焦点附近。位置敏感探测器靠定位销32和底面定位在转接板34上，再通过底部的M4螺孔夹紧。用内六角螺钉33将转接板固定在一维手动平移台35的工作台上，平移台的底座固定在L型连接板30上，通过调节平移台的调整杆可以调整敏感探测器在镜头光轴方向的位移，确保在测量区域内投射在PSD感应屏上的光斑明亮清晰。相机调整是在测量之前进行，一旦内外参数测量或三维测量开始相机的各结构保持不变。The function of the two-dimensional PSD camera is to project the measurement target light source onto the PSD sensing screen to form a spot, and output the coordinates of the center of the spot. Its principle is similar to that of a digital camera, so it is named after it. The structure of the two-dimensional PSD camera designed by the present invention is as shown in Figure 7, and a threaded hole is processed on the wall of the L-shaped connecting plate 30, and the size specifications are determined according to the required industrial lens interface model, generally C-type and F-type Or CS type, in this design, a fixed-focus C-mount industrial lens with a focal length of 25mm is used29. The C10443-03 type position sensitive detector 31 of Japan Hamamatsu Company is placed behind the L-shaped connecting plate 30 to ensure that the sensing screen is located near the rear focus of the lens. The position sensitive detector is positioned on the adapter plate 34 by the positioning pin 32 and the bottom surface, and then clamped through the M4 screw hole at the bottom. Fix the adapter plate on the workbench of the one-dimensional manual translation platform 35 with the hexagon socket head screw 33, the base of the translation platform is fixed on the L-shaped connecting plate 30, and the sensitive detector can be adjusted by adjusting the adjustment rod of the translation platform. The axial displacement ensures that the light spot projected on the PSD sensing screen in the measurement area is bright and clear. The camera adjustment is carried out before the measurement, and once the internal and external parameter measurement or three-dimensional measurement starts, the structures of the camera remain unchanged.

(2) 二维PSD相机内外参数测量(2) Measurement of internal and external parameters of two-dimensional PSD camera

二维PSD相机的三维测量方法的工作原理是小孔成像，光是沿直线传播的，目标光源发出的光线经过透镜后照射到PSD感应屏上，感应屏上的感应点与光源中心成一条直线。PSD感应屏是一个二维平面，坐标系原点在其几何中心。如图2所示，设测量目标所在的坐标系为世界坐标系W，镜头坐标系L、图像坐标系I、 PSD坐标系S参照图2所示。设空间P点在世界坐标系W下得到坐标为P (X_W，Y_W，Z_W)，其对应的投影点P’在PSD坐标系S下得坐标为P’(u,v)，二者之间的转换关系为：The working principle of the three-dimensional measurement method of the two-dimensional PSD camera is small hole imaging. The light propagates along a straight line. The light emitted by the target light source passes through the lens and then shines on the PSD sensing screen. The sensing point on the sensing screen is in a straight line with the center of the light source. . The PSD sensor screen is a two-dimensional plane, and the origin of the coordinate system is at its geometric center. As shown in FIG. 2 , assume that the coordinate system where the measurement target is located is the world coordinate system W, and the lens coordinate system L, the image coordinate system I, and the PSD coordinate system S are shown in FIG. 2 . Assume that the coordinates of point P in the space are P (X _W , Y _W , Z _W ) in the world coordinate system W, and the coordinates of the corresponding projected point P' in the PSD coordinate system S are P'(u,v), two The conversion relationship between them is:

${Z Z}_{L L} [\begin{matrix} μ μ \\ ν ν \\ 11 \end{matrix}] = = [\begin{matrix} {f f}_{x x} & 00 & {μ μ}_{00} & 00 \\ 00 & {f f}_{y the y} & {ν ν}_{00} & 00 \\ 00 & 00 & 11 & 00 \end{matrix}] \cdot \cdot [\begin{matrix} R R & T T \\ 00 & 11 \end{matrix}] \cdot \cdot [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \\ 11 \end{matrix}] = = [\begin{matrix} A A & 00 \end{matrix}] \cdot &Center Dot; [\begin{matrix} R R & T T \\ 00 & 11 \end{matrix}] \cdot \cdot [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \\ 11 \end{matrix}] - - - - - - ((11))$

内外参数的测量参考张氏平面标定法，设世界坐标系W的原点在模板左上角光源的中心，水平方向为X轴，垂直方向为Y轴，垂直于平面向里为Z轴，则Z_W=0：The measurement of internal and external parameters refers to Zhang’s plane calibration method. Assume that the origin of the world coordinate system W is at the center of the light source in the upper left corner of the template, the horizontal direction is the X axis, the vertical direction is the Y axis, and the vertical direction is the Z axis, then Z _W =0:

${Z Z}_{L L} [\begin{matrix} μ μ \\ ν ν \\ 11 \end{matrix}] = = [\begin{matrix} {f f}_{x x} & 00 & {μ μ}_{00} & 00 \\ 00 & {f f}_{y the y} & {ν ν}_{00} & 00 \\ 00 & 00 & 11 & 00 \end{matrix}] \cdot \cdot [\begin{matrix} {R R}_{11} & {R R}_{22} & T T \\ 00 & 00 & 11 \end{matrix}] \cdot &Center Dot; [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ 11 \end{matrix}] = = H h \cdot &Center Dot; [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ 11 \end{matrix}] - - - - - - ((22))$

上式中，R₁、R₂分别为旋转矩阵R的第一和第二列。在标定时，空间目标点为平面模板上的光源中心点，PSD坐标系中的投影点坐标去PSD输出值。内外参数的求解过程采用张氏标定法，具体步骤如下：In the above formula, R ₁ and R ₂ are the first and second columns of the rotation matrix R, respectively. During calibration, the spatial target point is the center point of the light source on the plane template, and the PSD output value is obtained from the projection point coordinates in the PSD coordinate system. The process of solving internal and external parameters adopts Zhang’s calibration method, and the specific steps are as follows:

①相机内参数的测量如图3所示，制作一张平面模板37，在上面均匀布置散射点光源阵列36，其水平和垂直间隔均相等，且精度较高，如30mm。点光源的数量尽量要多，如9×9以上，尽量使水平方向和垂直方向的数量相等，以保证两个方向上的测量精度相等。在距离模板一定距离处放置PSD相机，使相机视野覆盖所有光源。①Measurement of internal parameters of the camera As shown in Figure 3, a plane template 37 is made, and scattered point light source arrays 36 are evenly arranged on it, with equal horizontal and vertical intervals and high precision, such as 30mm. The number of point light sources should be as large as possible, such as more than 9×9, and the number in the horizontal direction and the vertical direction should be equal as much as possible to ensure that the measurement accuracy in the two directions is equal. Place the PSD camera at a certain distance from the template so that the camera field of view covers all light sources.

②固定好相机和模板的位置后，启动光源阵列控制电路从左上角起扫描式依次点亮光源，同时记录下每个点光源相对应的PSD的输出坐标值，在此步骤进行中要保证PSD相机和模板方位不变；② After fixing the positions of the camera and the template, start the light source array control circuit to light up the light sources in a scanning manner from the upper left corner, and record the output coordinates of the PSD corresponding to each point light source. During this step, ensure that the PSD The orientation of the camera and the template remains unchanged;

③模板上最后一个光源点测量完毕后，将光源点在世界坐标系的坐标和相应投影点在PSD感应屏上的坐标代入下述方程中：③After the measurement of the last light source point on the template is completed, substitute the coordinates of the light source point in the world coordinate system and the coordinates of the corresponding projection point on the PSD sensing screen into the following equation:

$[\begin{matrix} {X x}_{W W} & {Y Y}_{W W} & 11 & 00 & 00 & 00 & - - μ μ \cdot \cdot {X x}_{W W} & - - μ μ \cdot &Center Dot; {X x}_{W W} & - - μ μ \\ 00 & 00 & 00 & {X x}_{W W} & {Y Y}_{W W} & 11 & - - ν ν \cdot \cdot {X x}_{W W} & - - ν ν \cdot &Center Dot; {Y Y}_{W W} & - - ν ν \end{matrix}] \cdot &Center Dot; [\begin{matrix} {h h}_{1111} \\ {h h}_{1212} \\ {h h}_{1313} \\ {h h}_{21 twenty one} \\ {h h}_{22 twenty two} \\ {h h}_{21 twenty one} \\ {h h}_{3131} \\ {h h}_{3232} \\ {h h}_{3333} \end{matrix}] = = [\begin{matrix} 00 \\ 00 \end{matrix}] - - - - - - ((33))$

每个光源点对应上式两个方程，5个以上的测量点就可以通过最小二乘法求解单应性矩阵H。Each light source point corresponds to the above two equations, and more than 5 measurement points can solve the homography matrix H by the least square method.

④改变相机或者模板的方位，改变幅度尽可能大、全面，重复进行步骤②，至少改变4次方位，由于旋转矩阵为标准正交矩阵，根据其性质可得下述方程：④Change the orientation of the camera or template, and the change range should be as large and comprehensive as possible. Repeat step ②, and change the orientation at least 4 times. Since the rotation matrix is an orthonormal matrix, the following equation can be obtained according to its properties:

$[\begin{matrix} {V V}_{1212}^{T T} \\ {(({V V}_{1111} - - {V V}_{22 twenty two}))}^{T T} \end{matrix}] \cdot &Center Dot; b b = = 00 - - - - - - ((44))$

其中in

$V_{ij} = {[\begin{matrix} h_{i 1} \cdot h_{j 1} & h_{i 1} \cdot h_{j 2} + h_{i 2} \cdot h_{j 1} & h_{i 1} \cdot h_{j 3} + h_{i 3} \cdot h_{j 1} & h_{i 2} \cdot h_{j 2} & h_{i 2} \cdot h_{j 3} + h_{i 3} \cdot h_{j 2} & h_{i 3} \cdot h_{j 3} \end{matrix}]}^{T}$ ，b中含有6个元素，分别对应对称矩阵B的六个元。 $V_{ij} = {[\begin{matrix} h_{i 1} &Center Dot; h_{j 1} & h_{i 1} &Center Dot; h_{j 2} + h_{i 2} \cdot h_{j 1} & h_{i 1} \cdot h_{j 3} + h_{i 3} \cdot h_{j 1} & h_{i 2} \cdot h_{j 2} & h_{i 2} &Center Dot; h_{j 3} + h_{i 3} &Center Dot; h_{j 2} & h_{i 3} &Center Dot; h_{j 3} \end{matrix}]}^{T}$ , b contains 6 elements, corresponding to the six elements of the symmetric matrix B.

相机的每一个方位可以得到两个方程，改变4个以上方位就可以通过最小二乘法求出b，即求出了相机内参数矩阵A。Two equations can be obtained for each orientation of the camera, and b can be obtained by the least square method by changing more than 4 orientations, that is, the camera internal parameter matrix A is obtained.

⑤根据以下关系求解旋转矩阵的前两列和平移矩阵：⑤ Solve the first two columns of the rotation matrix and the translation matrix according to the following relationship:

然后通过将R₁和R₂通过正交扩展出R₃，求出相机外参数矩阵[R T]。Then R ₃ is obtained by extending R ₁ and R ₂ orthogonally to obtain the camera extrinsic parameter matrix [RT].

(3) 目标光源三维坐标测量方法(3) Three-dimensional coordinate measurement method of target light source

世界坐标系W与PSD坐标S可以简化表示为：The world coordinate system W and PSD coordinate S can be simplified as:

$k k [\begin{matrix} μ μ \\ ν ν \\ 11 \end{matrix}] = = [\begin{matrix} {m m}_{11} & {m m}_{22} & {m m}_{33} & {m m}_{44} \\ {m m}_{55} & {m m}_{66} & {m m}_{77} & {m m}_{88} \\ {m m}_{99} & {m m}_{1010} & {m m}_{1111} & {m m}_{1212} \end{matrix}] \cdot &Center Dot; [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \\ 11 \end{matrix}] = = M m \cdot &Center Dot; [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \\ 11 \end{matrix}] - - - - - - ((66))$

上式中，M称为转化矩阵，k为比例系数，其与相机到标定板之间的距离称正比。为了便于求解将k消去得：In the above formula, M is called the transformation matrix, and k is the proportional coefficient, which is proportional to the distance between the camera and the calibration plate. In order to facilitate the solution, k is eliminated:

$[\begin{matrix} {m m}_{11} - - {m m}_{99} μ μ & {m m}_{22} - - {m m}_{1010} μ μ & {m m}_{33} - - {m m}_{1111} μ μ \\ {m m}_{55} - - {m m}_{99} ν ν & {m m}_{66} - - {m m}_{1010} ν ν & {m m}_{77} - - {m m}_{1111} ν ν \end{matrix}] * * [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \end{matrix}] = = [\begin{matrix} {m m}_{1212} μ μ - - {m m}_{44} \\ {m m}_{1212} ν ν - - {m m}_{88} \end{matrix}] - - - - - - ((77))$

当空间一点在单个PSD相机投影过程中可以列出一个如上式方程，三维空间点坐标有三个未知量，上述方程式只有两个方程，故使用两个PSD相机就能求解出空间点的三维坐标值。When a point in space is projected by a single PSD camera, one of the above equations can be listed. The coordinates of the three-dimensional space point have three unknown quantities. The above equation has only two equations, so the three-dimensional coordinate value of the space point can be solved by using two PSD cameras. .

(4) 双PSD相机测量装置(4) Dual PSD camera measuring device

双PSD相机测量装置是将两个PSD相机安装在一个多自由度的平台上，构成一套适合多种测量环境、测量范围可调的三维测量系统。多自由度平台用于调整PSD相机的空间位姿，不仅可以实现相机垂直、水平移动，而且还可以通过两个旋转平台实现水平观测角和俯仰角的调整，极大的拓宽了相机的视野，适合应用于更复杂工况下的三维测量。The dual PSD camera measurement device is to install two PSD cameras on a multi-degree-of-freedom platform to form a three-dimensional measurement system suitable for various measurement environments and with adjustable measurement range. The multi-degree-of-freedom platform is used to adjust the spatial pose of the PSD camera. It can not only move the camera vertically and horizontally, but also adjust the horizontal observation angle and pitch angle through two rotating platforms, which greatly broadens the field of view of the camera. It is suitable for 3D measurement in more complex working conditions.

双PSD相机测量装置的整体结构图如图6所示，这种设计可以使PSD相机实现空间4个自由度调整。底座23用于支撑调整平台、与被测量设备连接从而固定测量装置的作用。带断电保护的Z轴电控运动平台28通过螺栓固定在底的立板上，通过电机17和丝杠18来调整固定在承载台24上的相机垂直位移。由于测量的工况的特殊性，可能要求两个PSD相机在竖直方向上的位置并不完全相同，鉴于此，在滑动承载工作台24上安装两个可调整高度差的滑轨21，它由两部分组成，一部分用于与Z轴平台28相连接，另一部分用于相机水平移动的燕尾型导轨。连接部分有两个通槽，用六角头螺栓22穿过通槽旋入承载台上的螺孔内，从而将滑轨21紧固在承载台上，通过改变通槽与螺栓的相对位置来调整滑轨21在承载台面上的位置，从而形成了两个PSD相机的高度差。导轨部分由一个水平的燕尾槽构成，其与小托板20相配合，使单个PSD相机可以在水平方向调整位置。相机的水平角度和俯仰角的改变通过手动调整旋转平台25和角位平台26实现。左PSD相机27和右PSD相机19的公共视野就是双PSD相机测量装置的有效测量区域。The overall structure diagram of the dual PSD camera measurement device is shown in Figure 6. This design can make the PSD camera realize the adjustment of 4 degrees of freedom in space. The base 23 is used to support the adjustment platform and connect with the measured equipment so as to fix the measuring device. The Z-axis electronically controlled motion platform 28 with power-off protection is fixed on the vertical plate at the bottom by bolts, and the vertical displacement of the camera fixed on the carrying platform 24 is adjusted by the motor 17 and the lead screw 18 . Due to the particularity of the measured working conditions, it may be required that the positions of the two PSD cameras are not exactly the same in the vertical direction. In view of this, two slide rails 21 with adjustable height difference are installed on the sliding bearing workbench 24, which It consists of two parts, one part is used to connect with the Z-axis platform 28, and the other part is used for the dovetail guide rail for the horizontal movement of the camera. There are two through slots in the connecting part, and the hexagon head bolts 22 are screwed into the screw holes on the bearing platform through the through slots, so as to fasten the slide rail 21 on the bearing platform, and adjust by changing the relative positions of the through grooves and the bolts. The position of the slide rail 21 on the carrying table forms a height difference between the two PSD cameras. The guide rail part is formed by a horizontal dovetail groove, which cooperates with the small supporting plate 20, so that the position of a single PSD camera can be adjusted in the horizontal direction. The change of the horizontal angle and pitch angle of the camera is realized by manually adjusting the rotating platform 25 and the angular position platform 26 . The common field of view of the left PSD camera 27 and the right PSD camera 19 is the effective measurement area of the dual PSD camera measurement device.

(5)换刀机械手运动轨迹的跟踪测量(5) Tracking measurement of the trajectory of the tool changing manipulator

自动换刀系统是加工中心刀库的重要组成部分，如图4所示。其功能将加工使用刀具自动装夹到主轴上，并把主轴上不再使用的刀具卸载放回刀库中。机身1是ATC的支撑框架，机械手臂2的主要运动由沿回转轴13的滑动和绕回转轴的转动构成，这两个分运动靠弧面凸轮5控制，摆杆9上的端面滑块6随凸轮端面的成型槽滑动，从而通过传递杆10带动机械手臂滑动。与机械手臂相连接的轴套3通过花键与回转轴13配合，周面转块11随凸轮周面的成型槽转动并由传递轴8传给主锥齿轮11，经变速后由次锥齿轮12带动回转轴13和机械手臂2转动。为了精确地测量机械手臂的运动轨迹，如图5所示，将双PSD测量装置16放置在凸轮顶壳4上，在机械手臂的两端对称布置两个大功率散射点光源14和15作为测量光源。在测量过程的每个时刻必然有一个测量光源在机械手臂的下方运动，将其点亮而对应的另一个光源关闭，双PSD相机测量装置实时测量点亮光源的三维坐标，并通过软件拟合出测量目标点的运动轨迹。The automatic tool change system is an important part of the tool magazine of the machining center, as shown in Figure 4. Its function automatically clamps the tools used for processing to the spindle, and unloads the unused tools on the spindle and puts them back into the tool magazine. The fuselage 1 is the supporting frame of the ATC. The main movement of the mechanical arm 2 is composed of sliding along the rotary axis 13 and rotation around the rotary axis. These two sub-movements are controlled by the arc cam 5. 6 slides with the forming groove on the end surface of the cam, thereby driving the mechanical arm to slide through the transmission rod 10 . The shaft sleeve 3 connected with the mechanical arm cooperates with the rotary shaft 13 through a spline, and the peripheral surface turning block 11 rotates with the forming groove on the cam peripheral surface, and is transmitted to the main bevel gear 11 by the transmission shaft 8, and is transferred by the secondary bevel gear after speed change. 12 drives rotary shaft 13 and mechanical arm 2 to rotate. In order to accurately measure the motion track of the mechanical arm, as shown in Figure 5, the double PSD measuring device 16 is placed on the cam top shell 4, and two high-power scattering point light sources 14 and 15 are symmetrically arranged at both ends of the mechanical arm as the measurement light source. At each moment of the measurement process, there must be a measurement light source moving under the mechanical arm, which is turned on and the corresponding other light source is turned off. The dual PSD camera measurement device measures the three-dimensional coordinates of the light source in real time and fits it through software The motion track of the measurement target point.

1）双PSD相机测量装置安装方位调整的具体操作步骤如下：1) The specific operation steps for adjusting the installation orientation of the dual PSD camera measuring device are as follows:

①将测量装置放置在合适的测量位置上并将其固定，如图5所示。测量装置安装平面尽可能选择光滑平整的设备大表面，以便于固定，本发明选在大连高金数控集团生产的MDH-80卧式加工中心的凸轮外壳4上表面。调整测量装置的位置，使所有调整测量目标处于镜头的最小成像距离以外，同时又要确保测量目标的活动范围在镜头的景深内。调整测量装置的安装角度，使测量目标的整体轨迹处于测量装置的调整范围内。调整完毕后使用磁座将装置底座13固定；①Place the measuring device in a suitable measuring position and fix it, as shown in Figure 5. The installation plane of the measuring device should be as smooth and level as possible, so as to facilitate fixing. The present invention chooses the upper surface of the cam housing 4 of the MDH-80 horizontal machining center produced by Dalian Gaojin Numerical Control Group. Adjust the position of the measurement device so that all adjustment measurement targets are outside the minimum imaging distance of the lens, and at the same time ensure that the range of motion of the measurement target is within the depth of field of the lens. Adjust the installation angle of the measurement device so that the overall trajectory of the measurement target is within the adjustment range of the measurement device. After the adjustment is completed, use the magnetic base to fix the device base 13;

②竖直方向调整两相机位置。首先调整Z轴运动平台28，本发明采用卓立汉光的PSA150-11-Z，将左右PSD相机的光轴移动到测量目标整体运动轨迹的中心附近，然后松开固定滑轨的螺栓，调整两个相机的高度差，使在垂直方向上相机的视野包含整个机械手臂下半周的运动轨迹；②Adjust the positions of the two cameras vertically. First adjust the Z-axis motion platform 28, the present invention adopts the PSA150-11-Z of Zhuoli Hanguang, and moves the optical axes of the left and right PSD cameras to the center of the overall motion track of the measurement target, then loosen the bolts that fix the slide rail, and adjust The height difference between the two cameras makes the field of view of the camera in the vertical direction include the movement trajectory of the entire lower half of the robotic arm;

③根据测量目标运动范围与测量装置的距离调整小托板20和旋转平台25，来改变两PSD相机的水平方位，使每个相机的测量范围尽可能大，调整完后将小托板和旋转平台锁紧，本发明采用的旋转平台为联英精机的SX120-73；③Adjust the small supporting plate 20 and the rotating platform 25 according to the distance between the measurement target movement range and the measuring device to change the horizontal orientation of the two PSD cameras, so that the measurement range of each camera is as large as possible. After the adjustment, the small supporting plate and the rotating platform The platform is locked, and the rotating platform used in the present invention is the SX120-73 of Lianying Precision Machinery;

④调整相机的俯仰角。调整角位平台26的旋杆来改变相机的俯仰角，保证机械手臂在水平位置时，两个PSD相机的公共视野能同时包含1号和2号LED光源，本发明使用的角位平台26为联英精机的SJ110-15。④ Adjust the pitch angle of the camera. Adjust the rotating rod of angle platform 26 to change the pitch angle of camera, when guaranteeing that mechanical arm is in horizontal position, the public visual field of two PSD cameras can comprise No. 1 and No. 2 LED light sources simultaneously, and angle platform 26 used in the present invention is Lianying Seiki's SJ110-15.

⑤小范围微调整相机的空间位姿，使被测量目标的运动范围充满有效测量视野的95%以上。⑤ Micro-adjust the spatial pose of the camera in a small range, so that the motion range of the measured target fills more than 95% of the effective measurement field of view.

2）运动轨迹测量的具体操作步骤如下：2) The specific operation steps of motion track measurement are as follows:

①当机械手臂的位置处于图5所示时开始测量，首先点亮1号LED光源，关闭2号LED光源，接下来的半个循环周期内1号LED光源都处于回转轴的下方，处于测量装置的测量范围内。同时，以一定的频率采集PSD输出的坐标值；①When the position of the robot arm is as shown in Figure 5, the measurement is started. First, the No. 1 LED light source is turned on, and the No. 2 LED light source is turned off. In the next half cycle, the No. 1 LED light source is under the rotation axis and is in the measurement position. within the measuring range of the device. At the same time, the coordinate value of the PSD output is collected at a certain frequency;

②半个工作周期结束后，1号LED光源处于图5所示的2号LED光源的位置，此时关闭1号光源，点亮2号光源，重复步骤①；② After the half working cycle is over, the No. 1 LED light source is in the position of No. 2 LED light source shown in Figure 5. At this time, turn off the No. 1 light source, light up the No. 2 light source, and repeat step ①;

③将左右PSD相机的输出的坐标对代入下述方程中：③Substitute the output coordinate pairs of the left and right PSD cameras into the following equation:

$[\begin{matrix} {m m}_{11 l l} - - {m m}_{99 l l} {μ μ}_{l l} & {m m}_{22 l l} - - {m m}_{1010 l l} {μ μ}_{l l} & {m m}_{33 l l} - - {m m}_{1111 l l} {μ μ}_{l l} \\ {m m}_{55 l l} - - {m m}_{99 l l} {ν ν}_{l l} & {m m}_{66 l l} - - {m m}_{1010 l l} {ν ν}_{l l} & {m m}_{77 l l} - - {m m}_{1111 l l} {ν ν}_{l l} \\ {m m}_{11 r r} - - {m m}_{99 r r} {μ μ}_{r r} & {m m}_{22 r r} - - {m m}_{1010 r r} {μ μ}_{r r} & {m m}_{33 r r} - - {m m}_{1111 r r} {μ μ}_{r r} \\ {m m}_{55 r r} - - {m m}_{99 r r} {ν ν}_{r r} & {m m}_{66 r r} - - {m m}_{1010 r r} {ν ν}_{r r} & {m m}_{77 r r} - - {m m}_{1111 r r} {ν ν}_{r r} \end{matrix}] \cdot &Center Dot; [\begin{matrix} {X x}_{W W} \\ {Y Y}_{W W} \\ {Z Z}_{W W} \end{matrix}] = = [\begin{matrix} {m m}_{1212 l l} {μ μ}_{l l} - - {m m}_{44 l l} \\ {m m}_{1212 l l} {ν ν}_{l l} - - {m m}_{88 l l} \\ {m m}_{1212 r r} {μ μ}_{r r} - - {m m}_{44 r r} \\ {m m}_{1212 r r} {ν ν}_{r r} - - {m m}_{88 r r} \end{matrix}] - - - - - - ((88))$

每一对坐标对使用最小二乘法求解方程得到一个目标点的三维坐标值，这样就可以将ATC的运动轨迹转化为一系列的离散点。Each pair of coordinates uses the least square method to solve the equation to obtain a three-dimensional coordinate value of a target point, so that the trajectory of the ATC can be converted into a series of discrete points.

④使用样条曲线函数将离散的空间点拟合成一条运动曲线；④ Use the spline curve function to fit the discrete space points into a motion curve;

⑤根据所测量出的运动曲线分解加工动作，通过与设计曲线对比评价刀库ATC的可靠性。⑤ Decompose the machining action according to the measured motion curve, and evaluate the reliability of the tool magazine ATC by comparing with the design curve.

本发明的算法的实现依靠软件编程，其编程的思路如图8的程序流程图所示，首先标定相机内参数，然后调整相机位置和固定标定模板。再标定左右相机外参数并选择统一坐标点，在统一坐标原点的基础上，进行空间点坐标计算。最后，拟合轨迹曲线。The realization of the algorithm of the present invention relies on software programming, and the idea of programming is shown in the program flow chart of Fig. 8. Firstly, the internal parameters of the camera are calibrated, and then the camera position is adjusted and the calibration template is fixed. Then calibrate the extrinsic parameters of the left and right cameras and select a unified coordinate point, and calculate the spatial point coordinates on the basis of the unified coordinate origin. Finally, fit the trajectory curve.

本发明一种用于测量ATC机械手臂运动轨迹方法和装置，可以实时在线跟踪记录机械手臂的运动状态。该方法是光学非接触式测量，ATC机构的结构一般比较复杂，结构封闭性强，不便于采用接触式测量。采用光学非接触式测量方式不会干预ATC的动作，采用光点的实时提取，测量目标为LED光源，安装简单方便，适用范围广。传统的的非接触式视觉测量是基于提取工件形貌特征来确定空间姿态，这种方法提取速度与精度是两个矛盾对立面，往往为了提高精度而降低了提取速度，所以只能应用于离线测量，本发明采用PSD提取目标光点坐标的方式，响应速度快，满足实时在线测量要求；通过机械手臂的运动轨迹可以判断换刀过程中的定位精度、运动平稳性等运动状态，为刀库可靠性检测提供量化方法。The invention discloses a method and device for measuring the motion trajectory of an ATC mechanical arm, which can track and record the motion state of the mechanical arm on-line in real time. This method is an optical non-contact measurement. The structure of the ATC mechanism is generally more complicated, and the structure is strong, so it is not convenient to use contact measurement. The optical non-contact measurement method will not interfere with the action of the ATC, and the real-time extraction of light points is adopted. The measurement target is an LED light source. The installation is simple and convenient, and the application range is wide. The traditional non-contact visual measurement is based on extracting the shape features of the workpiece to determine the spatial attitude. The extraction speed and accuracy of this method are two contradictory opposites. The extraction speed is often reduced in order to improve the accuracy, so it can only be applied to offline measurement. , the present invention uses PSD to extract the coordinates of the target light spot, which has a fast response speed and meets the requirements of real-time on-line measurement; the motion state such as positioning accuracy and motion stability during the tool change process can be judged through the motion trajectory of the mechanical arm, which is reliable for the tool magazine. Sex detection provides quantitative methods.

Claims

1. A method for measuring the motion track of an automatic tool changing mechanical arm is characterized by comprising the following steps: the method is a non-contact three-dimensional online measurement method based on two-dimensional PSD, a light source serving as a measured target is arranged on an ATC mechanical arm to be measured, the light source is projected on a two-dimensional sensing screen of a PSD camera consisting of two position sensitive detectors through a lens to form a light spot, the two PSD cameras respectively output two-dimensional coordinate values under the excitation of the light spot, a voltage value corresponding to the coordinate position is output after passing through a signal processor, a computer collects and stores corresponding signals through a voltage collection card, three-dimensional coordinate values of the measured light source are back calculated by the two sets of collected two-dimensional coordinate values, and finally, a series of discrete points are calculated according to high-frequency collected data to form a motion track of the mechanical arm; the method comprises the following specific steps:

1) measurement of internal and external parameters of PSD camera

Setting the coordinate system of the target light source to be measured as the world coordinate system W, the coordinate system of the projected light spot as the PSD coordinate system S, and setting a certain point P (X) under the world coordinate system W_W，Y_W，Z_W) The coordinate of the corresponding projection point P 'in the PSD coordinate system S is P' (u, v), and the transformation relationship between the two is:

where A is an internal parameter matrix containing the focal length f and the offset (u)₀,v₀) R, T are the rotation matrix and translation matrix from the world coordinate system to the lens coordinate system, [ R T ] respectively]An external parameter matrix of the PSD camera;

using a flat template 37 to measure the internal and external parameters of a PSD camera, set Z_w=0；

Solving internal and external parameters according to a Zhang calibration method, starting a control circuit (38) after fixing a PSD camera and a template, sequentially lighting light sources in a scanning mode from the upper left corner of a light source array (36), simultaneously acquiring corresponding PSD output coordinate values, and substituting the acquired data and the corresponding coordinate values of the light sources in a world coordinate system into the following equation until the last light source measurement is completed:

when the number of light sources in the plane template (37) is enough, a homography matrix H is solved by a least square method, different homography matrixes can be solved by changing the direction of the camera or the template, and a plurality of groups of H are substituted into the following equation:

wherein

B is a vector formed by six independent elements of the symmetric matrix B;

when the number of changed directions is enough, solving b by a least square method, inversely calculating a parameter matrix A in the camera, and substituting A into the following formula:

R₁=A^-1H₁，R₂=A^-1H₂，T=A^-1H₃(5)

then by adding R₁And R₂By orthogonal expansion of R₃To find out the external parameter matrix [ R T]；

2) Calculation of three-dimensional coordinates of target light points

The light beam emitted by the scattering target light source is converged by the lens and then projected on the PSD camera induction screen, and the target light source center coordinate (X) is under the world coordinate system_W，Y_W，Z_W) The relation with the projection light spot center coordinates (mu, v) under the PSD coordinate system is as follows:

wherein,

two equations of the above formula can be listed by using two-dimensional coordinates output by a single PSD camera, the two PSD cameras simultaneously sense the same target light source, four equations can be listed, and coordinate values of the center of the target light source under a world coordinate system are solved by a least square method;

3) measuring movement track of mechanical arm

Mounting a target light source on a mechanical arm of a machining center ATC, forming a motion track of the mechanical arm in space along with tool changing action, and measuring the motion track by adopting a three-dimensional coordinate value calculation and discrete point curve fitting method of the target light source; the sensing surface of the PSD camera is a two-dimensional plane, the two-dimensional coordinate value of the projection light spot and the three-dimensional coordinate value of the target light spot are represented by an internal parameter matrix and an external parameter matrix of the PSD camera, the three-dimensional coordinates of the target point can be reversely solved by the coordinate values output by the two-dimensional PSD cameras, and the motion trail of the mechanical arm in the machining process is fitted by a series of discrete target points.

2. The method for measuring the motion trail of the automatic tool changing mechanical arm according to claim 1, wherein the method comprises the following steps: the measuring device adopted by the method is a measuring device with two PSD cameras as cores and multiple degrees of freedom; the measuring device is provided with displacement adjusting mechanisms in the vertical direction and the horizontal direction and also provided with a rotating angle and pitching adjusting mechanism, and the device has the following specific structure:

a bolt hole is processed on a vertical plate of a base (23), a Z-axis electric control platform (28) with power-off protection is fixed on the vertical plate through a bolt, a motor (17) drives a bearing workbench (24) to move in the vertical direction through a ball screw (18) through a displacement adjusting mechanism in the vertical direction, two slide rails (21) with adjustable height difference are installed on the sliding bearing workbench (24) through a group of bolts (22), a horizontal dovetail guide rail at the top of each slide rail is matched with a small supporting plate (20), and the position of a PSD camera on the small supporting plate (20) in the horizontal direction is adjusted through the displacement adjusting mechanism in the horizontal direction; a rotating platform (25) for adjusting the rotating angle and a displacement platform (26) for adjusting the pitch angle are sequentially assembled on the small supporting plate (20), and a left PSD camera (27) and a right PSD camera (19) are respectively installed on a workbench of the angular displacement platform (26);

the PSD camera overall structure consists of a focusing part, a position sensitive detector, a fine adjustment part and a connecting frame; the focusing part uses an industrial fixed-focus lens (29), the size and luminosity of a projection light spot on a PSD induction screen are changed by adjusting an aperture in the lens, the industrial lens (29) is connected with an L-shaped connecting plate (30) through a standard C port, a position sensitive detector (31) is connected with a signal processor by adopting a 10-pin communication cable, the position sensitive detector (31) is arranged on a workbench of a manual micro-feeding platform (35), the distance between the position sensitive detector (31) and the industrial lens (29) can be adjusted by adjusting the micro-feeding platform (35), the L-shaped connecting plate (30) and the positioning adapter plate (34) form a connecting frame part, the position sensitive detector (31) is positioned and clamped on the positioning adapter plate (34) through the positioning pin (32) and the threads on the bottom surface of the position sensitive detector (31), and the positioning adapter plate (34) is fixed on a workbench of the one-dimensional manual translation table (35) through the inner hexagon screw (33).