CN205066775U - High accuracy movement track detection device - Google Patents

Abstract

The utility model provides a high accuracy movement track detection device, wherein: the three -dimensional gesture angle of moving part is measured by inertial sensing locator system, three -dimensional position of moving part and three -dimensional gesture angle are measured by electromagnetism locator system, the three -dimensional positional information of moving part measures in machine vision locator system, an output that is used for data fusion's treater to connect inertial sensing locator system, electromagnetism locator system, machine vision locator system simultaneously to fuse the data of three subsystem, obtain moving part's movement track. The utility model discloses it single and receive the problem that environmental factor disturbed easily to overcome locating information volume that single system obtained.

Description

A high-precision motion trajectory detection device

technical field

The utility model relates to a motion track detection device, in particular to a high-precision motion track detection device composed of inertial sensing, machine vision and electromagnetic positioning subsystems. The application field includes the motion track detection of moving parts such as industrial robots.

Background technique

The increasingly widespread use of moving parts such as industrial robots and mobile robots has led to high requirements for their operational performance, especially for the dynamic positioning accuracy of motion actuators. For example, an industrial robot is a semi-closed-loop motion control method realized by a reducer, a servo motor, an incremental encoder, and a load feedback unit. (end tool displacement) will affect path positioning accuracy. Therefore, a high-precision motion trajectory detection system to realize the real-time motion feedback and control of the robot is very necessary for applications such as assembly positioning, vibration analysis, and performance index measurement and evaluation.

At present, research on motion tracking and positioning technology at home and abroad is relatively concentrated on radio frequency signal detection and positioning, inertial sensing, magnetic field positioning, visual positioning and sound source positioning. The Xsens motion capture system based on inertial sensing technology fuses information from accelerometers, gyroscopes and magnetometers to obtain high-precision three-dimensional attitude information. Because the acceleration value has a large error after secondary integration, the obtained linear displacement is only can be used as a reference value. The electromagnetic positioning system detects the three-dimensional magnetic field intensity of the permanent magnet or electromagnetic coil in space through the magnetic sensor array, and then iteratively solves the spatial position and attitude information of the permanent magnet or electromagnetic coil. NDI's electromagnetic positioning system uses two The vertically placed 3-axis electromagnetic induction coil realizes complete 6-axis motion detection, but the electromagnetic positioning system is susceptible to interference from environmental electromagnetic waves and ferromagnetic substances, which is unavoidable in industrial environments; VICON motion capture system based on optical positioning technology It consists of an infrared high-speed camera and a data processor. The infrared high-speed camera captures passive luminous marking points, and uses the principle of machine vision and laser scanning technology to measure the movement position information. However, the optical positioning system can only measure the spatial position information of the marking points. And is susceptible to occlusion as well as ambient light and background.

Aiming at the problem that the optical positioning system is easily blocked, a Chinese utility model patent application with the application number 201420695742.6 proposes a laser tracker target ball positioning device based on inertial detection, which can realize the function of disconnection and connection, and is convenient for difficult positioning. Measurement of measuring points or occlusion positions, but mainly not to improve the accuracy and dimension of target positioning and tracking.

Therefore, in a complex and changeable test environment, the use of a single positioning measurement system will have the following disadvantages: 1. The amount of information obtained from the measurement is single. For example, the optical positioning system can only measure the position information, and the inertial positioning system can only measure the attitude information. ; 2. The positioning accuracy is not high due to the interference of environmental factors. For example, the electromagnetic positioning system is easily disturbed by electromagnetic waves, and the optical positioning system is easily affected by occlusion, ambient light and background.

Utility model content

In order to overcome the problems of a single positioning system, the purpose of this utility model is to provide a high-precision motion trajectory detection device to realize a high-precision, high-stability and fast motion detection system for 6-dimensional poses of moving parts such as industrial robots.

In order to solve the above technical problems, the utility model adopts the following technical solutions:

A high-precision motion trajectory detection device, including an inertial sensor positioning subsystem, an electromagnetic positioning subsystem, a machine vision positioning subsystem, and a processor for data fusion, wherein:

The inertial sensing positioning subsystem has an input end to measure the three-dimensional attitude angle of the moving part, and an output end connected to the processor for data fusion;

The electromagnetic positioning subsystem, its input end measures the three-dimensional position and three-dimensional attitude angle of the moving part, and the output end is connected to the processor for data fusion;

The machine vision positioning subsystem, its input end measures the three-dimensional position information of the moving parts, and the output end is connected to the processor for data fusion;

The processor for data fusion is simultaneously connected to the output terminals of the inertial sensor positioning subsystem, the electromagnetic positioning subsystem, and the machine vision positioning subsystem, and fuses the data of the three subsystems to obtain the motion trajectory of the moving parts.

Preferably, the inertial sensing positioning subsystem includes a MEMS sensor and a first sub-processor, the MEMS sensor is attached to the moving part and realizes the acquisition of real-time three-dimensional attitude information, and the output end of the MEMS sensor is connected to the first sub-processor The processor, the first sub-processor fuses the data collected by the MEMS sensor to obtain accurate three-dimensional attitude angle information of the moving part.

Preferably, the MEMS sensor includes a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer, and the three-axis gyroscope, three-axis accelerometer, and three-axis magnetometer are all connected to the first sub-processor.

Preferably, the electromagnetic positioning subsystem includes a three-axis orthogonal excitation coil and a three-axis orthogonal induction coil, and a second sub-processor, wherein: the induction coil is fixed on the moving part, and the excitation coil is used as a fixed point ; The excitation coil alternately passes through the alternating current of the same frequency and amplitude, and the alternating current signal generates an alternating electromagnetic field in space through the excitation coil, and the induction coil outputs a signal with the same frequency in the alternating electromagnetic field; the second sub-processor according to the induction The amplitude and phase information of the output signal of the coil can be used to obtain the position and direction information of the induction coil relative to the excitation coil.

Preferably, the machine vision positioning subsystem is made up of several cameras and FPGA embedded processors, wherein: several cameras are installed around the feature points for continuously collecting the image signals of the feature points from different orientations in real time and Output to the FPGA embedded processor; the feature points adopt active or passive luminous marking points, and are attached to the moving parts; the FPGA embedded processor is used to control the camera to obtain the image signal containing the marking point, and the image signal Perform processing to realize the acquisition of the image coordinates of the feature points.

Compared with the prior art, the utility model has the beneficial effects:

The device of the utility model includes three subsystems, and each subsystem is provided with a corresponding processor, so that the generated local positioning tracking trajectory is transmitted to the processor for data fusion through the serial communication bus, and the processor for data fusion The data of the subsystems are fused to obtain the trajectory of the moving parts. The utility model adopts three sub-systems to collect multi-source information, improves detection accuracy and dimension by means of multi-source information, and overcomes the problem that the amount of positioning information obtained by a single system is single and easily interfered by environmental factors.

Furthermore, the device not only has local independent position tracking capabilities (design of three subsystems), but also has global monitoring and evaluation features. Through the multi-source information obtained in each positioning subsystem and further fusion, the six-dimensional pose (three-dimensional position and three-dimensional attitude angle) of industrial robots and other moving parts can be detected with high precision, high stability and fast motion.

Description of drawings

Other features, objects and advantages of the present invention will become more apparent by reading the detailed description of non-limiting embodiments with reference to the following drawings:

Fig. 1 is the structural block diagram of the motion locus detection device of an embodiment of the present invention;

Fig. 2 is a schematic structural diagram of an inertial sensing positioning subsystem of an embodiment of the present invention;

Fig. 3 is a schematic structural diagram of the electromagnetic positioning subsystem of an embodiment of the present invention;

Fig. 4 is a schematic structural diagram of a machine vision positioning subsystem according to an embodiment of the present invention;

In the figure: moving part 1, inertial sensor positioning subsystem 2, MEMS sensor 21, electromagnetic positioning subsystem 3, induction coil 31, excitation coil 32, machine vision positioning subsystem 4, camera 41, feature point 42, FPGA embedded Processor 43, processor 5 for data fusion.

detailed description

The utility model is described in detail below in conjunction with specific embodiments. The following examples will help those skilled in the art to further understand the utility model, but do not limit the utility model in any form. It should be noted that those skilled in the art can make several modifications and improvements without departing from the concept of the present utility model. These all belong to the protection domain of the present utility model.

As shown in Figure 1, a high-precision motion trajectory detection device is composed of an inertial sensor positioning subsystem 2, an electromagnetic positioning subsystem 3, a machine vision positioning subsystem 4, and a processor 5 for data fusion; in:

The inertial sensing positioning subsystem 2 has an input end that measures the three-dimensional attitude angle of the moving parts, and an output end that is connected to the processor for data fusion;

The electromagnetic positioning subsystem 3, its input end measures the three-dimensional position and three-dimensional attitude angle of the moving part, and the output end is connected to the processor for data fusion;

The machine vision positioning subsystem 4, its input end measures the three-dimensional position information of the moving parts, and the output end is connected to the processor for data fusion;

The processor 5 for data fusion is simultaneously connected to the output terminals of the inertial sensor positioning subsystem, the electromagnetic positioning subsystem, and the machine vision positioning subsystem, and fuses the data of the three subsystems to obtain the motion trajectory of the moving parts .

In this embodiment, the data of each subsystem is transmitted to the processor 5 for data fusion through a serial communication bus (SPI or CAN) to realize data fusion.

The processor 5 for data fusion, its specific fusion technology can adopt the existing technology, in a preferred embodiment, also can adopt the fusion structure model based on the distributed state, carry out coordinate transformation and data correction, data association to the data combined with state estimation. The processor 5 used for data fusion can effectively utilize the positioning redundant information and complementary information of multiple positioning systems, comprehensively consider the influence of noise interference and environmental factors, improve the accuracy and dimension of target positioning and tracking, and enhance the reliability of the system and robustness.

Taking the end of the flange plate of the mechanical arm as the moving part 1 for measurement, the detailed embodiments of the utility model will be described in conjunction with the accompanying drawings.

As shown in Figure 2, in a preferred embodiment, the described inertial sensing positioning subsystem 2 includes a MEMS sensor 21 and a first sub-processor, the MEMS sensor 21 is attached to the moving part 1 and realizes real-time three-dimensional posture information To obtain, the output end of the MEMS sensor 21 is connected to the first sub-processor, and the first sub-processor fuses the data collected by the MEMS sensor to obtain accurate three-dimensional attitude angle information of the moving part.

In this embodiment, the inertial sensor positioning subsystem 2 is fixed at the end of the flange of the robot arm (moving part 1), and the first sub-processor processes the data of the MEMS sensor 21 to obtain the flange of the robot arm (moving part 1). 1) Accurate three-dimensional attitude angle information of the end of the moving part. The specific data processing of the first sub-processor adopts existing technologies, such as Kalman filter or orthogonal cosine matrix fusion algorithm, etc., which do not belong to the innovation of the present invention, and the innovation of the present invention lies in providing the overall structure of the device.

Further, the MEMS sensor 21 includes a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer, and the three-axis gyroscope, three-axis accelerometer, and three-axis magnetometer are all connected to the first sub-processing device. The data collected by the three-axis gyroscope, the three-axis accelerometer, and the three-axis magnetometer are all transmitted to the first sub-processor, and the first sub-processor processes the data of the three to obtain the current attitude angle of the MEMS sensor 21. 21 is attached to the moving part 1, so it is also the attitude angle of the moving part 1.

Preferably, the three-axis gyroscope measures the angular acceleration and obtains the attitude angle of the deflection of the object after one integration; the three-axis magnetometer measures the geomagnetic intensity to obtain the heading angle of the object; the three-axis accelerometer measures the three-axis gravity component for measuring Absolute pitch angle and roll angle (relative to the earth coordinates); the dynamic performance of the attitude angle output by the three-axis magnetometer and the three-axis accelerometer is poor, which is used to compensate the attitude angle obtained after one integration of the three-axis gyroscope signal and remove the drift.

The first sub-processor can be realized by logic circuit, integrated circuit, single-chip microcomputer, etc., and its specific processing technology can be realized by using existing technology.

As shown in Figure 3, in another preferred embodiment, the electromagnetic positioning subsystem is composed of a three-axis orthogonal excitation coil 32, a three-axis orthogonal induction coil 31, and a second sub-processor, wherein: induction The coil 31 is fixed at the end of the flange (moving part 1) of the mechanical arm, and the exciting coil 32 is used as a fixed point. In a short period of time, the excitation coil 32 alternately passes through the alternating current of the same frequency and amplitude, so that the excitation coil 32 generates an alternating electromagnetic field in space, and the induction coil 31 outputs signals with the same frequency in the alternating electromagnetic field; The processor processes and obtains position and direction information of the induction coil 31 relative to the excitation coil 32 according to the amplitude and phase information of the output signal of the induction coil 31 .

Described second sub-processor can adopt logic circuit, integrated circuit, single-chip microcomputer etc. to realize, and its specific processing technology can adopt existing technology to realize, certainly also can adopt following technology in a preferred embodiment:

Assume that the central position of the excitation coil 32 is (a, b, c), the central position of the induction coil 31 is (x, y, z) and its direction relative to the excitation coil 32 uses three rotation angles (α, β, γ) means that the potential amplitude EM is a function of relative position parameters (x-a, y-b, z-c) and angle parameters (α, β, γ), that is, EM=f(x-a, y-b, z-c, α, β, γ ). Therefore, six or more independent combinations of excitation coils 32 and induction coils 31 are used to sample the potential signals of induction coils 31 under different excitation coils 32, and six position parameters (x-a, y-b, x-a, y-b, z-c) and angle parameters (α, β, γ) values.

As shown in Figure 4, in another preferred embodiment, described machine vision localization subsystem comprises several cameras 41 and FPGA embedded processor 43, wherein: several cameras 41 are installed around feature point 42, camera 41 The collected image data is transmitted to the FPGA embedded processor for processing; the feature point 42 can be an active luminescent or passive luminous marking point, and is attached to the moving part. The FPGA embedded processor 43 is used to control the camera to acquire image signals containing marker points, and transmit the image signals to the embedded DSPbuilder module for processing, so as to realize the acquisition of image coordinates of feature points.

Described FPGA embedded processor 43, its specific processing technology can adopt prior art to realize, certainly also can adopt following technology in a preferred embodiment: first a plurality of cameras 41 are collected the image of feature point 42 continuously in real time from different orientations Signal; each channel image signal adopts the target recognition based on the color space model, that is, take the average value of the feature point for many times, extract the color component model [R, G, B] of the feature point 42, and then combine the feature point 42 Each color component of the image to be recognized is compared, and then the two-dimensional coordinates of the feature point 42 in the image are found; the two-dimensional coordinates of the imaging position of the feature point 42 at different times in each camera 41 are 2D through an optimization algorithm. For coordinate interpolation calculation, the optimization algorithm can use the least square method, average method or median method; multiple spatially different straight lines formed by multiple two-dimensional plane coordinates in all camera 41 imaging planes are calculated by three-dimensional coordinate positioning algorithm. The three-dimensional coordinates of point 42 in space.

The data of above-mentioned each subsystem is transmitted to the processor 5 that is used for data fusion, and described second sub-processor can adopt existing product to realize, such as data fusion processor, single-chip microcomputer etc., its specific data fusion technology can adopt existing Technical realization, certainly also can adopt following technology in a preferred embodiment: the processor 5 that is used for data fusion adopts distributed state fusion structure model, and the characteristic of this structure model is that the sensor data of each subsystem enters and is used for data fusion In front of the processor 5, the local positioning tracking track is generated by its own data processor (i.e. the first sub-processor, the second sub-processor, FPGA embedded processor), and then the processed information is sent to the Processor 5 for data fusion. The processor 5 for data fusion performs coordinate conversion, data correction, data association, and state estimation fusion according to the positioning and tracking trajectory data of each subsystem, and finally generates a target positioning with a 6-dimensional pose. track track. In addition, the target positioning and tracking trajectory data is also fed back to each subsystem to provide reference and calibration for the positioning and tracking of each subsystem. The processor 5 used for data fusion can effectively utilize the positioning redundant information and complementary information of multiple positioning systems, comprehensively consider the influence of noise interference and environmental factors, improve the accuracy and dimension of target positioning and tracking, and enhance the reliability of the system and robustness.

The device of the utility model is equipped with three positioning subsystems, and the multi-source information obtained by these three positioning subsystems is processed later, so that the three-dimensional positions and three-dimensional attitude angles of industrial robots and other moving parts can be realized with high precision, high stability and high accuracy. Fast motion detection. The utility model provides a detection device with a reasonable structure, in which the data processing technology adopted by each part does not belong to the protection content of the utility model.

The specific embodiments of the present utility model have been described above. It should be understood that the utility model is not limited to the above-mentioned specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the utility model.

Claims (5)

1. a high-precision motion track detection device, is characterized in that, comprises inertia sensing positioning subsystem, electromagnetic location subsystem, machine vision positioning subsystem and the processor for data fusion, wherein:

Described inertia sensing positioning subsystem, its input end measuring moving component three-dimension altitude angle, output terminal is connected to the described processor for data fusion;

Described electromagnetic location subsystem, its input end measuring moving component three-dimensional position and three-dimension altitude angle, output terminal is connected to the described processor for data fusion;

Described machine vision positioning subsystem, its input end measuring moving component three dimensional local information, output terminal is connected to the described processor for data fusion;

The described processor for data fusion connects the output terminal of inertia sensing positioning subsystem, electromagnetic location subsystem, machine vision positioning subsystem simultaneously, and the data of three subsystems is merged, and obtains the movement locus of moving component.

2. a kind of high-precision motion track detection device according to claim 1, it is characterized in that, described inertia sensing positioning subsystem comprises MEMS sensor and the first sub-processor, MEMS sensor is attached at moving component and realizes the acquisition of real-time three-dimensional attitude information, the output terminal of described MEMS sensor is connected to the first sub-processor, the data of the first sub-processor to MEMS sensor collection merge, and obtain the accurate 3 d pose angle information of moving component.

3. a kind of high-precision motion track detection device according to claim 2, it is characterized in that, described MEMS sensor comprises three-axis gyroscope, three axis accelerometer, three axle magnetometers, and described three-axis gyroscope, three axis accelerometer, three axle magnetometers are all connected to the first sub-processor.

4. a kind of high-precision motion track detection device according to any one of claim 1-3, it is characterized in that: described electromagnetic location subsystem comprises the orthogonal drive coil of three axles and the orthogonal inductive coil of three axles and the second sub-processor, wherein: inductive coil is fixed on moving component, drive coil is then as point of fixity; Drive coil is alternately through the alternating current of same frequency and amplitude, and the current signal of alternation produces the electromagnetic field of alternation in space by drive coil, and inductive coil is at the identical signal of the elect magnetic field output frequency of alternation; The amplitude that second sub-processor outputs signal according to inductive coil and phase information, obtain inductive coil relative to the position of drive coil and directional information.

5. a kind of high-precision motion track detection device according to any one of claim 1-3, it is characterized in that, described machine vision positioning subsystem is made up of several cameras and FPGA flush bonding processor, wherein: several cameras are arranged on around unique point, for exporting to FPGA flush bonding processor from the picture signal of different orientation real-time collection and continual collection unique points; Described unique point adopts the gauge point of active illuminating or passive luminescence, and is attached on moving component; FPGA flush bonding processor obtains for controlling camera the picture signal containing gauge point, and picture signal is processed, the acquisition of realization character dot image coordinate.