Six-degree-of-freedom flexible adaptive integrated large-scale connector automatic docking method based on vision and force feedback

Abstract

The invention provides a six-degree-of-freedom flexible adaptive integrated large-scale connector automatic docking method based on vision and force feedback. According to the method, a parallel structure is used as a design basis, and the difficulty of automatic docking is solved by combining an active compliance thought of taking a force sensor as feedback and a thought of taking a passive compliance docking mechanism as mechanism passive compliance docking. A set of active and passive compliant control integrated large-scale connector automatic docking system based on combination of vision and force feedback is designed. As a research focus in the robotics field, the active compliance control technology can be deployed in numerous accurate docking application scenes depending on force feedback, the active compliance docking strategy is formulated for the design difficulty of the active compliance docking control, and the strategy is simple, fast, low in cost and high in reliability. The anti-interference performance and the reliability of the control system are greatly improved, the system is suitable for automatic docking occasions in the industrial automation field in some complex environments, and contributions are made to further research and expansion of the technology. The high-precision flexible butt joint technology is applied more and more widely nowadays with the continuous development of science and technology, compared with a traditional butt joint device, the automatic butt joint device designed by the invention is higher in assembly efficiency and precision and higher in intelligent degree, damage to all paths of plug-in devices is smaller, meanwhile, economic losses can be further reduced, and the automatic butt joint device is suitable for popularization and application. And human resource cost is saved.

Description

Technical Field

[0001] The present invention relates to the field of automation engineering technology, and in particular to a six-degree-of-freedom compliant and self-adaptive integrated large-scale connector automatic docking system based on vision and force feedback. Background Art

[0002] In automation engineering applications, a large device needs to complete corresponding tasks between different workstations, and when performing experimental operations at each workstation, the device must maintain a stable connection with the main control system, which includes but is not limited to power supply, network communication, control instructions, measurement data, monitoring information, and pneumatic systems.

[0003] An integrated automatic docking device that can automatically dock in an unmanned environment and in reverse is of great significance. The device requires optical fiber modules, network communication modules, air path docking joints, aviation connectors and other connectors to automatically dock in an integrated manner without damaging the connectors on the docking substrate. However, most of the automatic docking devices in the existing technology adopt a passive and flexible docking method and require manual intervention. They are generally suitable for situations where there are small posture deviations, so the application scenarios are limited. Secondly, most of the automatic docking devices that adopt an active and flexible method are small in scale and cannot automatically dock a large number of large connector substrates, and generally use laser trackers or position sensors. The existing technology has the following disadvantages:

[0004] 1) Laser trackers are expensive and cannot be widely promoted. Secondly, this method requires placing targets on the parts to be inspected and is generally used for calibration.

[0005] 2) When the external environment changes, such as in complex environments such as low temperature and low pressure, the sensor is easily affected by the environment and causes instability in the final use. Therefore, this type of device is difficult to use in complex environments;

[0006] 3) Since there is no force feedback element on the substrate, the control system based on position feedback alone cannot fully guarantee the safety and reliability of the terminal connector during docking. The terminal connector on the substrate is easily damaged due to excessive contact force during docking, which not only makes maintenance difficult but also affects the final docking timeliness, and the degree of intelligence is not high.

[0007] Therefore, there is an urgent need to develop a six-degree-of-freedom compliant and adaptive integrated large-scale connector automatic docking system based on vision and force feedback. Summary of the Invention

[0008] The purpose of the present invention is to provide a six-degree-of-freedom compliant and adaptive integrated large-scale connector automatic docking system based on vision and force feedback to solve the problems existing in the prior art.

[0009] The technical solution adopted to achieve the purpose of the present invention is as follows: a six-degree-of-freedom flexible and adaptive integrated large connector automatic docking method based on vision and force feedback, and the automatic docking device includes a receiving end and several sets of supply ends.

[0010] The receiving end includes a receiving end frame, a receiving end docking substrate, and an active compliant docking mechanism. The receiving end frame is a rectangular frame with open ends and a hollow interior. A base is provided below the receiving end frame. The receiving end frame is fixedly mounted on the ground via the base. The open side of the receiving end frame facing the supply end is sealed with the receiving end docking substrate. The receiving end docking substrate is provided with a receiving end guide pin hole and a camera avoidance hole on its surface.

[0011] The active and flexible docking mechanism is arranged in the inner cavity of the receiving end frame. The active and flexible docking mechanism includes 6 drive modules. Each drive module includes a ball joint I, a connecting rod, a ball joint II and a linear drive module. The linear drive modules are arranged on the four side walls of the receiving end frame. Among them, two linear drive modules are arranged on the upper wall of the receiving end frame, two linear drive modules are arranged on the lower wall, one linear drive module is arranged on the left wall, and one linear drive module is arranged on the right wall. Each linear drive module has a guide rail slider. The ball joint end surface mounting surface of the ball joint I is fixedly connected to the guide rail slider. The ball joint mounting surface of the ball joint II is fixed on the receiving end docking base plate. The connecting rod spans between the corresponding ball joint I and ball joint II. The ends of the connecting rod are fixedly connected to the rotatable ends of the ball joint I and ball joint II respectively. The 6 drive modules constitute a 6-pss mechanism.

[0012] The supply end includes a supply end frame, a supply end base, a docking substrate, a passive compliant docking mechanism and a supply end actuator. The supply end frame is a rectangular frame with open ends and a hollow interior. The supply end docking substrate is arranged at the open side of the supply end frame facing the receiving end. The passive compliant docking mechanism is arranged in the inner cavity of the supply end frame. The supply end docking substrate has a supply end guide pin and a visual camera on the side facing the receiving end, and an adaptive plate is arranged on the side facing away from the receiving end. A three-dimensional force sensor is arranged at the rear end of the supply end guide pin. Two passive compliant guide holes and a spherical top column are arranged on the adaptive plate. The spherical top column is arranged between the two passive compliant guide holes.

[0013] The passively compliant docking mechanism includes a linear drive mechanism, a base, and a passively compliant guide pin. The base is fixedly connected to the supply-end frame. Two return-to-zero guide rails are arranged on the base. Sliders are slidably mounted on the return-to-zero guide rails. The passively compliant guide pins are fixedly connected to the slides. The linear drive mechanism can drive the slides to move along the return-to-zero guide rails.

[0014] The supply-side base is mounted on the ground. The supply-side actuator is placed on the supply-side base. The supply-side actuator includes a linear drive mechanism and a receiving plate. The linear drive mechanism is capable of driving the receiving plate to move. The supply-side docking substrate and the supply-side frame are both placed on the receiving plate. The linear drive mechanism drives the supply-side docking substrate to move toward the receiving-side docking substrate.

[0015] During operation, there is a posture deviation between the supply-side device and the receiving-side device. The automatic docking system uses a visual camera and a three-dimensional force sensor to identify the current posture and then controls the active compliant docking mechanism, performing coarse posture adjustment based on visual feedback and fine posture adjustment based on force feedback. During the posture adjustment process, the supply-side actuator drives the supply-side docking substrate toward the receiving end, ultimately completing the automatic docking of the supply-side docking substrate and the receiving-end docking substrate. At the initial docking moment, the passive compliant guide pin is tightly embedded in the passive compliant guide hole. The supply-side docking substrate is in a fixed, zero-degree-of-freedom state. When automatic docking is initiated, the supply-side actuator drives the supply-side docking substrate toward the receiving-end docking substrate. When the supply-side guide pin shaft enters the receiving-end guide pin hole by the set length, the linear drive mechanism of the passive compliant docking mechanism drives the passive compliant guide pin and then retracts it to zero, placing the adaptive plate in a six-degree-of-freedom state and, in turn, the supply-side docking substrate in a freely docked state. The coupled motion of the supply-side actuator and the passive, compliant docking mechanism eliminates positional deviations between the supply-side and receiving-side docking substrates. By controlling the linear positions of the six drive module guide rails, the spatial position of the receiving-side docking substrate is controlled, ultimately achieving flexible, active docking of the supply-side and receiving-side docking substrates.

[0016] Furthermore, the supply end docking substrate is provided with an air path connector, a high current connector, an optical fiber module, a low current module and a network communication module.

[0017] Furthermore, a receiving end circuit connector, a receiving end gas path connector and a receiving end signal path connector corresponding to the supply end docking substrate are provided on the board surface of the receiving end docking substrate.

[0018] Furthermore, a baffle is provided. The baffle cover is provided on the periphery of the adaptive plate. The baffle is provided with a hollow portion for the passive compliant guide pin to pass through.

[0019] Furthermore, the docking method specifically includes the following steps:

[0020] 1) Perform rough position adjustment based on visual feedback. Once the receiving end reaches the initial docking position, the automatic docking system begins operation. Using images captured by an industrial camera, preliminary position recognition is performed to calculate the relative position deviation between the supply-side docking substrate and the receiving-side docking substrate in their current position. The active compliant docking mechanism performs preliminary visual-based position adjustment on the docking substrate. After the active compliant docking mechanism completes the adjustment, the supply-side actuator activates, driving the supply end forward.

[0021] 2) Repeat step 1) until the industrial camera's field of view disappears.

[0022] 3) Precise posture adjustment based on force feedback.

[0023] 4) After rough and fine adjustments to the posture, the pin shaft on the supply side smoothly enters the pin hole on the receiving side, and the active and flexible docking mechanism completes the assembly of the pin shaft into the pin hole.

[0024] 5) The passive compliant guide pin is driven by the driving motor to retreat in the Z direction. As the supply-end docking substrate moves forward in the Z direction, the baffle contacts the spherical top column on the supply-end docking substrate. In the subsequent automatic docking process, the supply-end docking substrate can follow in five degrees of freedom in space except for the linear motion in the Z direction.

[0025] Furthermore, in step 1), the captured image is processed, characteristic circles are identified based on the captured images of the two pin holes, and the coordinates of the points of the characteristic circles are calculated. Finally, the posture is solved, and the obtained target posture is transmitted to the automatic docking control system. The system solves the inverse position solution of each slider of the active compliant docking mechanism based on the target posture solution. The inverse kinematic solution formula of the mechanism is as follows:

[0026]

[0027] Where, l i is the connecting rod length, z Bi is the z coordinate of the spherical joint Bi in the global coordinate system.

[0028] The automatic docking system then controls the corresponding linear drive module to move to the specified position, completing the initial posture adjustment of the six-degree-of-freedom active compliant docking device in the visual feedback stage.

[0029] Furthermore, step 3) specifically includes the following sub-steps:

[0030] 3.1) Analyze the force information measured by the two 3D force sensors. Establish the sensor coordinates O-XYZ at the center of the two 3D force sensors, and the measured force coordinates O′-X′Y′Z′ at the front ends of the two pins. These coordinates are parallel and share the same Z axis. Let F be the force acting on the force sensor, the distance between the two coordinate systems (i.e., the length of the pin) be h, and the distance between pins 1 and 2 (i.e., the distance between 3D force sensors 1 and 2) be d. The following mechanical model can be established.

[0031]

[0032] 3.2) Identify the relative position relationship between the supply-side docking substrate and the receiving-side docking substrate in the current chamfer contact state.

[0033] 3.3) Identify the relative position relationship between the supply-end docking substrate and the receiving-end docking substrate in the current shaft-hole contact state.

[0034] 3.4) During the execution of steps 3.2) and 3.3), the active compliant docking mechanism is adjusted in posture and position.

[0035] Furthermore, when the device needs to be disconnected, the automatic docking system first controls the supply-side actuator to move backward, while monitoring the value of the position sensor installed on the supply-side docking base plate. When the rated value is reached, that is, the pin shaft has not completely disengaged from the pin hole, the automatic docking system controls the passive flexible docking mechanism to move, and the motor controls the passive flexible guide pin to move forward, causing it to fit tightly with the passive flexible guide hole, and fix the supply-side docking base plate and the supply-side box. Then the automatic docking system controls the actuator to quickly retreat to a safe disengagement position.

[0036] The technical effects of the present invention are unquestionable:

[0037] A. Based on the parallel structure, we focused on solving the difficulties of automatic docking by combining the idea of active compliance using force sensors as feedback and the idea of passive compliance docking using passive compliance docking mechanisms. We designed an integrated large-scale connector automatic docking system based on active and passive compliance control combined with vision and force feedback.

[0038] B. As a research focus in the field of robotics, active compliant control technology can be deployed in numerous precise docking applications that rely on force feedback. This present invention addresses the design difficulties of active compliant docking control by developing an active compliant docking strategy. This strategy is simple, fast, and low-cost, significantly improving the control system's anti-interference and reliability. It is suitable for automated docking in complex industrial automation environments and contributes to the further research and expansion of this technology.

[0039] C. With the continuous development of science and technology, high-precision flexible docking technology is being used more and more widely. Compared with traditional docking devices, the automatic docking device designed in this invention has higher assembly efficiency and precision, a higher degree of intelligence, and less damage to various connector components. At the same time, it can also further reduce economic losses and save human resource costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 This is the overall structure diagram of the integrated large-scale connector automatic docking device;

[0041] Figure 2 This is a schematic diagram of the passive flexible docking structure of the integrated large-scale connector automatic docking device; Figure 2 a and Figure 2 b shows different views of the passive compliant docking structure;

[0042] Figure 3 This is a schematic diagram of the active and flexible docking structure of the integrated large-scale connector automatic docking device;

[0043] Figure 4 This is a schematic diagram of the docking substrate on the supply side of the integrated large-scale connector automatic docking device;

[0044] Figure 5 This is a schematic diagram of the actuator on the supply side of the integrated large-scale connector automatic docking device;

[0045] Figure 6 This is the flow chart of the automatic docking system posture adjustment during the visual feedback phase;

[0046] Figure 7 This is the flow chart of the automatic docking system posture adjustment during the force feedback phase;

[0047] Figure 8 This is the block diagram of the posture control principle in the visual feedback stage;

[0048] Figure 9 This is the principle block diagram of posture control in the force feedback stage;

[0049] Figure 10 This is a block diagram of the position-based impedance control principle in the force feedback stage;

[0050] Figure 11 This is a schematic diagram of force solution for a three-dimensional force sensor;

[0051] Figure 12 Schematic diagram of mechanical analysis of chamfer contact stage;

[0052] Figure 13 Schematic diagram of mechanical analysis during the in-hole contact stage.

[0053] In the figure: receiving end docking substrate 1, receiving end guide pin hole 101, camera avoidance hole 102, active flexible docking mechanism 2, ball joint I 201, connecting rod 202, ball joint II 203, supply end docking substrate 3, supply end guide pin shaft 301, adaptive plate 302, passive flexible guide hole 3021, spherical top column 3022, high current connector 303, optical fiber module 304, low current module 305, network communication module 306, air path connector 307, passive flexible docking mechanism 4, passive flexible guide pin 404, slider 405, return to zero guide rail 407, supply end actuator 5, accommodating plate 501, linear drive actuator 502, visual camera 6. DETAILED DESCRIPTION

[0054] The present invention will be further described below with reference to the following examples, but it should not be understood that the scope of the present invention is limited to the following examples. Without departing from the above technical ideas of the present invention, various substitutions and modifications can be made according to common technical knowledge and customary means in the art, and all should be included in the scope of protection of the present invention.

[0055] Example 1:

[0056] The complex docking environment and the required posture adjustments during device docking pose significant challenges to the design of automatic docking devices. This embodiment provides a six-degree-of-freedom, compliant, adaptive, integrated large-scale connector automatic docking method based on vision and force feedback. The automatic docking device includes a receiving end and several sets of supply ends.

[0057] See also Figures 1 to 5 The receiving end includes a receiving end frame, a receiving end docking substrate 1, and an active flexible docking mechanism 2. The receiving end frame is a rectangular frame with open ends and a hollow interior. A base is provided below the receiving end frame. The receiving end frame is fixedly mounted on the ground via the base. The open side of the receiving end frame facing the supply end is sealed with the receiving end docking substrate 1. The receiving end docking substrate 1 is provided with a receiving end guide pin hole 101 and a camera avoidance hole 102 on its surface.

[0058] The active and flexible docking mechanism 2 is arranged in the inner cavity of the receiving end frame. The active and flexible docking mechanism 2 includes 6 drive modules. Each drive module group includes a ball joint I 201, a connecting rod 202, a ball joint II 203 and a linear drive module. The linear drive modules are arranged on the four side walls of the receiving end frame. Among them, two linear drive modules are arranged on the upper wall of the receiving end frame, two linear drive modules are arranged on the lower wall, one linear drive module is arranged on the left wall, and one linear drive module is arranged on the right wall. Each linear drive module has a guide rail slider. The ball joint end surface mounting surface of the ball joint I 201 is fixedly connected to the guide rail slider. The ball joint mounting surface of the ball joint II 203 is fixed on the receiving end docking base plate 1. The connecting rod 202 spans between the corresponding ball joint I 201 and ball joint II 203. The ends of the connecting rod 202 are fixedly connected to the rotatable ends of the ball joint I 201 and the ball joint II 203 respectively. Six drive modules constitute a 6-pss mechanism.

[0059] The supply end includes a supply end frame, a supply end base, a docking substrate 3, a passive and flexible docking mechanism 4 and a supply end actuator 5. The supply end frame is a rectangular frame with open ends and a hollow interior. The supply end docking substrate 3 is arranged at the open side of the supply end frame facing the receiving end. The passive and flexible docking mechanism 4 is arranged in the inner cavity of the supply end frame. The supply end docking substrate 3 has a supply end guide pin 301 and a visual camera 6 on the side facing the receiving end, and an adaptive plate 302 is arranged on the side facing away from the receiving end. A three-dimensional force sensor is arranged at the rear end of the supply end guide pin 301. Two passive and flexible guide holes 3021 and a spherical top column 3022 are provided on the adaptive plate 302. The spherical top column 3022 is arranged between the two passive and flexible guide holes 3021.

[0060] The passively compliant docking mechanism 4 includes a linear drive mechanism, a base, and passively compliant guide pins 404. The base is fixedly connected to the supply-end frame. Two return-to-zero guide rails 407 are arranged on the base. Sliders 405 are slidably mounted on the return-to-zero guide rails 407. The passively compliant guide pins 404 are fixedly connected to the slides 405. The linear drive mechanism can drive the slides 405 to move along the return-to-zero guide rails 407.

[0061] The supply-side base is mounted on the ground. The supply-side actuator 5 rests on the supply-side base. The supply-side actuator 5 comprises a linear drive mechanism and a receiving plate 501. The linear drive mechanism drives the receiving plate 501 to move. The supply-side docking substrate 3 and the supply-side frame are both resting on the receiving plate 501. The linear drive mechanism drives the supply-side docking substrate 3 to move toward the receiving-side docking substrate 1.

[0062] During operation, there is a posture deviation between the supply-end device and the receiving-end device. The automatic docking system uses the visual camera 6 and the three-dimensional force sensor to identify the current posture and then controls the active flexible docking mechanism 2, successively performing rough posture adjustment based on visual feedback and precise posture adjustment based on force feedback. During the posture adjustment process, the supply-end actuator 5 drives the supply-end docking substrate 3 to move in the direction of the receiving end, and finally completes the automatic docking of the supply-end docking substrate 3 and the receiving-end docking substrate 1. At the initial moment of docking, the passive flexible guide pin 404 is tightly embedded in the passive flexible guide hole 3021. The supply-end docking substrate 3 is in a zero-freedom fixed constraint state. After entering automatic docking, the supply-end actuator 5 drives the supply-end docking substrate 3 to move in the direction of the receiving-end docking substrate 1. After the supply-side guide pin 301 moves to the set length and enters the receiving-side guide pin hole 101, the linear drive mechanism of the passive compliant docking mechanism 4 drives the passive compliant guide pin 404 back to zero, placing the adaptive plate 302 in a six-degree-of-freedom state, thereby placing the supply-side docking substrate 3 in a free docking state. The coupled motion of the supply-side actuator 5 and the passive compliant docking mechanism 4 eliminates the positional deviation between the supply-side docking substrate 3 and the receiving-side docking substrate 1. By controlling the different linear positions of the guide rail sliders of the six drive modules, the position of the receiving-side docking substrate 1 in space is controlled, ultimately achieving flexible active docking of the supply-side docking substrate 3 and the receiving-side docking substrate 1 of the active compliant docking mechanism.

[0063] Example 2:

[0064] The main contents of this embodiment are the same as those of Embodiment 1, wherein the supply-side docking substrate 3 is provided with a gas circuit connector 307, a high-current connector 303, an optical fiber module 304, a low-current module 305, and a network communication module 306. The receiving-end docking substrate 1 is provided with a receiving-end circuit connector, a receiving-end gas circuit connector, and a receiving-end signal circuit connector corresponding to the supply-side docking substrate 3.

[0065] Example 3:

[0066] The main contents of this embodiment are the same as those of embodiment 1 or 2, wherein a baffle is further provided. The baffle cover is provided on the periphery of the adaptive plate 302. The baffle is provided with a hollow portion for the passive compliant guide pin 404 to pass through.

[0067] Example 4:

[0068] See also Figures 6 to 13 The main contents of this embodiment are the same as any one of embodiments 1 to 3, wherein the docking method specifically includes the following steps:

[0069] 1) Perform rough posture adjustment based on visual feedback. After the receiving end reaches the initial docking position, the automatic docking system starts working. The image taken by the industrial camera is used for preliminary posture recognition, and the relative posture deviation between the supply-end docking substrate and the receiving-end docking substrate in the current posture is calculated. The active flexible docking mechanism performs preliminary posture adjustment based on vision on the docking substrate. After the adjustment of the active flexible docking mechanism is completed, the supply-end actuator moves to drive the supply end forward. The captured image is processed, and the characteristic circle is identified based on the images of the two pin holes taken and the coordinates of the points of the characteristic circle are calculated. Finally, the posture is solved and the target posture is transmitted to the automatic docking control system. The system solves the inverse position of each slider of the active flexible docking mechanism based on the target posture. The kinematic inverse solution formula of the mechanism is as follows:

[0070]

[0071] Where, l i is the connecting rod length, z Bi is the z coordinate of the spherical joint Bi in the global coordinate system.

[0072] The automatic docking system then controls the corresponding linear drive module to move to the specified position, completing the initial posture adjustment of the six-degree-of-freedom active compliant docking device in the visual feedback stage.

[0073] 2) Repeat step 1) until the industrial camera's field of view disappears.

[0074] 3) Perform precise posture adjustment based on force feedback. Step 3) specifically includes the following sub-steps:

[0075] 3.1) Analyze the force information measured by the two 3D force sensors. Establish the sensor coordinates O-XYZ at the center of the two 3D force sensors, and the measured force coordinates O′-X′Y′Z′ at the front ends of the two pins. These coordinates are parallel and share the same Z axis. Let F be the force acting on the force sensor, the distance between the two coordinate systems (i.e., the length of the pin) be h, and the distance between pins 1 and 2 (i.e., the distance between 3D force sensors 1 and 2) be d. The following mechanical model can be established.

[0076]

[0077] 3.2) Identify the relative position relationship between the supply-side docking substrate and the receiving-side docking substrate in the current chamfer contact state.

[0078] 3.3) Identify the relative position relationship between the supply-end docking substrate and the receiving-end docking substrate in the current shaft-hole contact state.

[0079] 3.4) During the execution of steps 3.2) and 3.3), the active compliant docking mechanism is adjusted in posture and position.

[0080] 4) After rough and fine adjustments to the posture, the pin shaft on the supply side smoothly enters the pin hole on the receiving side, and the active and flexible docking mechanism completes the assembly of the pin shaft into the pin hole.

[0081] 5) The passive compliant guide pin is driven by the driving motor to retreat in the Z direction. As the supply-end docking substrate moves forward in the Z direction, the baffle contacts the spherical top column on the supply-end docking substrate. In the subsequent automatic docking process, the supply-end docking substrate can follow in five degrees of freedom in space except for the linear motion in the Z direction.

[0082] Example 5:

[0083] The main content of this embodiment is the same as any one of Embodiments 1 to 4, wherein, when the device needs to be disconnected, the automatic docking system first controls the supply-side actuator to move backward, while monitoring the value of the position sensor installed on the supply-side docking substrate. When the rated value is reached, that is, the pin shaft has not completely disengaged from the pin hole, the automatic docking system controls the passive flexible docking mechanism to move, and the motor controls the passive flexible guide pin to move forward, causing it to fit tightly with the passive flexible guide hole, and fix the supply-side docking substrate and the supply-side box. Then the automatic docking system controls the actuator to quickly retreat to a safe disengagement position.

[0084] Example 6:

[0085] The main content of this embodiment is the same as any one of Embodiments 1 to 5. In some specific engineering applications such as automated production lines and experimental centers, due to the particularity of the experimental environment, such as complex factors such as low temperature conditions, dry air and high concentrations of nitrogen, personnel cannot directly enter the experimental area to perform manual connection work. This embodiment addresses the demand for automatic docking of integrated large connectors in the field of industrial automation in such complex environments, and proposes a six-degree-of-freedom compliant and adaptive integrated large connector automatic docking method based on vision and force feedback.

[0086] See also Figure 1The automatic docking device mainly includes several sets of supply-side devices and one set of receiving-side devices. The supply-side device includes a passive flexible docking mechanism, a supply-side actuator, and a supply-side docking substrate; the receiving-side device includes an active flexible docking mechanism and a receiving-side docking substrate. In actual engineering applications, there is a posture deviation between the supply-side device and the receiving-side device. The automatic docking system uses an industrial camera installed on the supply-side docking substrate and a three-dimensional force sensor at the rear end of the pin shaft to identify the current posture and then control the active flexible docking mechanism in the receiving-side device, successively performing rough posture adjustment based on visual feedback and precise posture adjustment based on force feedback. During this period, the supply-side actuator under the supply-side device moves, driving the entire supply side forward, and finally completing the automatic docking of the supply-side docking substrate and the receiving-side docking substrate, completing the safe and reliable connection and disconnection of various types of connectors.

[0087] First, after the receiving end reaches the initial docking position, the automatic docking system starts working. In view of the large posture deviation between the current supply end and the receiving end, the system uses the image of the two pin holes on the receiving end taken by the industrial camera on the supply end docking substrate to perform preliminary posture recognition, calculate the relative posture deviation between the supply end docking substrate and the receiving end docking substrate in the current posture, and feed back the measurement data to the control system. The control system then calculates the movement amount of each linear drive module of the active flexible docking mechanism based on the current posture deviation, thereby performing a preliminary vision-based posture adjustment on the receiving end docking substrate. After the adjustment of the active flexible docking mechanism is completed, the supply end actuator moves, and the control system controls the execution servo motor to rotate slowly, driving the ball screw to rotate through the reducer, thereby driving the screw nut forward, and then driving the entire supply end device forward. Before the industrial camera's field of view completely disappears, repeat the above steps, that is, industrial camera recognition->posture adjustment->execution of servo motor rotation. The specific automatic docking system posture adjustment flow chart in the visual feedback stage is as follows: Figure 6 During this process, the automatic docking system Figure 8 The posture control principle diagram shown in the figure first sends instructions to the monocular vision system for image acquisition, then performs image processing on the captured image, identifies the characteristic circle based on the images of the two pin holes, and calculates the coordinates of the points of the characteristic circle. Finally, the posture is solved and the target posture is transmitted to the automatic docking control system. The system solves the inverse position solution of each slider of the active compliant docking mechanism based on the target posture solution. The inverse kinematic solution formula of the mechanism is shown below.

[0088]

[0089] Where, l i is the connecting rod length, z Bi is the z coordinate of the spherical joint Bi in the global coordinate system, and the rest are similar.

[0090] The automatic docking system then controls the corresponding linear drive module to move to the specified position, completing the initial posture adjustment of the six-degree-of-freedom active compliant docking device in the visual feedback stage.

[0091] Then, when the distance between the supply-side docking substrate and the receiving-side docking substrate is too close and the field of view of the industrial camera disappears, the automatic docking device enters the precise posture adjustment stage based on force feedback. The arrangement of the two three-dimensional force sensors in space and the force information measured are as follows: Figure 11 As shown in the figure, for ease of description, the sensor coordinates O-XYZ are established at the center of the two 3D force sensors, and the measured force coordinates O′-X′Y′Z′ are established at the front ends of the two pins. These coordinates are parallel and share the same Z axis. Let F be the force acting on the force sensor, the distance between the two coordinate systems (i.e., the length of the pin) be h, and the distance between pins 1 and 2 (i.e., the distance between 3D force sensors 1 and 2) be d. The following mechanical model can be established.

[0092]

[0093] Through the previous preliminary posture adjustment based on vision, the pin on the supply-side substrate can smoothly contact the chamfered part of the pin hole, thus entering the dual-axis hole chamfer contact stage. According to the measurement values of the two three-dimensional force sensors installed at the rear end of the pin s F x 、 s F y 、 s F z , when the shaft-hole assembly occurs as Figure 12 In the contact situation shown in (a), the axis of the pin is expressed as (0,d y ), then we can calculate F y1 <0, F z1 <0,F x1 = 0. Similarly, when the contact situation is Figure 12 As shown in (b), the axis of the pin shaft is expressed as (0, -d y ), the measured value of the force sensor is the contact force value F of the pin y1 >0, F z1 <0,F x1 = 0. When the chamfer contact state is Figure 12 (c) and Figure 12 In the state shown in (d), the axis coordinate of the pin is (-d x ,0) and (d x ,0), the same applies to the contact force value.

[0094] When the chamfer contact condition is Figure 12(e) As shown, the pin shaft has d relative to the pin hole. x and d y The deviation in the direction, taking the second quadrant deviation as an example, the contact force value at this time is the force sensor measurement value, and the axis of the pin in the axis coordinate of the shaft hole is expressed as (-d x ,d y ), then F can be calculated from the force sensor measurement value y1 <0, F x1 >0,F z1 <0. Similarly, the deviations of the other quadrants can also be judged by the positive and negative values of the force sensor measurement values. For example, the deviation of the pin axis in the first quadrant is (-d x ,-d y ), then the force sensor measured value F y1 >0, F x1 >0,F z1 <0. When the third quadrant deviation occurs, the axis of the pin shaft is expressed as (d x ,d y ), at this time the force sensor measurement value should meet F y1 <0, F x1 <0,F z1 <0. Similarly, when the fourth quadrant deviation occurs, the pin axis position is expressed as (d x ,-d y ), the force sensor measurement value at this time should satisfy F y1 >0, F x1 <0,F z1 <0. After identifying the contact point of the chamfer contact, it is also necessary to determine the specific angle at which the deviation occurs, such as Figure 12 As shown in (e), when the second quadrant deviation occurs, the angle deviation is 90°<Δ with the negative direction of the y-axis as the positive direction of the angle. θ <180°, the relationship between the force sensor measurement value and it can be calculated as:

[0095]

[0096] By analogy, when the chamfer contact state is the first quadrant deviation, the angle deviation value Δ of this state can be calculated by the measurement value of the force sensor. θ is arctan(F x1 / F y1 ); When the third quadrant deviation occurs, the angle deviation Δ is calculated by the force sensor measurement value θ =π+arctan(-F x1 / -F y1 ); When the chamfer contact state is the fourth quadrant deviation, the angle deviation Δ can be calculated θ For -arctan(-Fx1 / F y1 ). When the chamfer contact is a single point contact, the device may also have θ y Angular deviation in direction, considering θ y The size and positive and negative values of the values can be used to analyze single-point contact and two-point contact at the same time. y If the angle deviation is , if it is a single point contact, the pin that has chamfered contact may be pin 1 or pin 2. If it is a two-point contact, both pin 1 and pin 2 have chamfered contact. In both cases, the force sensor will return a pair of different sizes. s F x and s F z , by calculating the force sensor measurement value s M y The value of can be used to determine the posture state of the device at this time, that is,

[0097] s M y =-F x1 ·hF x2 h+F z1 d / 2-F z2 d / 2

[0098] When roll angle deviation occurs during chamfer contact, the force sensor returns a pair of unequal magnitudes. s F y The torque value of the supply end in the z direction is calculated as shown in the following formula:

[0099] s M z =-F y1 ·d / 2+F y2 d / 2

[0100] By calculating the torque value of the two supply end pins in the z direction, if s M z >0, that is, F y1 <F y2 , at this time, the roll angle deviation θ z <0; Similarly, if the calculated s M z <0, the roll angle deviation θ at this time z >0; if calculated s M z =0, then F y1 =F y2 , there is no roll angle deviation at this time.

[0101] According to the above identification method, the relative posture relationship between the supply-side docking substrate and the receiving-side docking substrate in the current chamfer contact state can be identified, as summarized in Table 1.

[0102] Table 1 Force and posture in chamfer contact state

[0103]

[0104]

[0105] Through the actuator stroke feedback, when the automatic docking system recognizes that a certain stroke has been reached, the precise posture adjustment based on the force feedback stage enters the hole contact state. When the supply end pin shaft and the receiving end docking substrate pin hole come into point contact, the force sensor measures the value. s F x and s F y The positive and negative value and size of the value are used to determine d x and d y The positive and negative of Δ θ Similarly, the force information of single-point contact when pin 1 or pin 2 is in contact in the hole can be obtained. When multiple-point contact occurs in the shaft-hole assembly, the multiple-point contact can be simplified into a combination of multiple single-point contact states, and then the force conditions are calculated. If pin 1 and pin 2 each have one or two points in contact with the corresponding pin hole in the hole contact state, the force information measured by the force sensor is the sum of the forces at the two points, that is:

[0106]

[0107] Analyze the possible posture deviation between the shaft and the hole. If there is a position offset between the two on the XOY plane, the resultant force of the force sensor measurements in the X and Y directions can be calculated. s F x as well as s F y Each offset situation corresponds to a type of force information. x and d y The positive and negative judgment of is similar to the chamfer contact situation, so I will not elaborate on it here. The above-mentioned in-hole contact situation is the contact state that exists under the theoretical state. After the pin shaft enters the pin hole and the posture deviation is adjusted, if there is still a value on the force sensor and the value is similar, then it is judged that the in-hole contact is a line contact state. Figure 13As shown. In this line contact state, the inner wall of the pin hole exerts a support force on the pin shaft in the XOY plane. The force exerted by the line contact can be regarded as a uniformly distributed force, and a composite force is established at the midpoint of the force-bearing part. In addition, due to the line contact, the friction coefficient of the inner wall of the pin hole cannot be ignored, so there is also a friction force in the Z direction on the pin shaft. At this time, considering the two pin shafts together, the force sensor measurement value is:

[0108]

[0109] Where μ1 is the friction coefficient of the inner wall of pin hole 1, and μ2 is the friction coefficient of the inner wall of pin hole 2.

[0110] When the pin has a position deviation in the X-axis or Y-axis direction relative to the pin hole, the mechanical analysis is similar to the chamfer contact state. The difference is that the force information of the two pins needs to be considered at the same time. If the pin as a whole has a positive deviation in the Y-axis relative to the pin hole, it can be inferred that s F y <0, s F x = 0. Similarly, when the pin has a negative deviation in the Y axis, a positive deviation in the X axis, and a negative deviation in the X axis relative to the pin hole s F y and s F x Similarly, if the position deviation of the pin relative to the pin hole is the first quadrant deviation, it can be deduced that s F y <0 and s F x >0, the force sensor can be deduced in the same way under the deviation of other quadrants s F y and s F x The sign of the value.

[0111] When the pin shaft has a pitch angle deviation θ relative to the pin hole in the Y-axis direction y When the force sensor is used to measure the s M y The sign of the value of s M y =-F x1 ·hF x2 h+F z1 d / 2-F z2 ·d / 2. If the supply end has an angle deviation of "down" relative to the receiving end, that is, θ y >0, we can calculate the current s M y >0; Similarly, when θ y <0, the force sensor measurement values M y <0.

[0112] When the pin shaft has a roll angle deviation θ relative to the pin hole in the Z-axis direction z When the force sensor is used to measure the s M z The sign of the value, where s M z =-F y1 ·d / 2+F y2 ·d / 2, the mechanical analysis at this time has a rolling angle deviation θ compared to the chamfered contact situation z The current shaft-hole contact state is identified based on the above-mentioned in-hole contact posture recognition method, and the summary is shown in Table 2.

[0113] Table 2 Force and posture under contact state in hole

[0114]

[0115]

[0116] In the precise posture adjustment stage based on force feedback, the automatic docking system follows Figure 7 The posture adjustment flow chart shown in the figure is used to adjust the posture of the active flexible docking mechanism. First, the posture is adjusted, and then the position is adjusted. Similarly, after calculating the displacement of each linear module, the expected position and the expected contact force are used as Figure 9 The force sensor value after each small adjustment and the position feedback of the motor encoder are used as the feedback quantity, and the force sensor value after each small adjustment and the position feedback of the motor encoder are used as the feedback quantity. Figure 10 The impedance control algorithm shown in the figure completes precise posture adjustment based on force feedback. In actual application, it is not difficult to find that the force sensor is difficult to achieve the theoretical contactless state, that is, the measurement value of the force sensor is. Therefore, in actual flexible control, in order to prevent the system from deteriorating in stability or even jittering, the force control rate of the force sensor measurement value is set as follows:

[0117]

[0118] Finally, after rough and fine adjustments to the posture, the supply-side pin shaft smoothly enters the receiving-side pin hole, and the active flexible docking mechanism completes the assembly of the pin shaft into the pin hole. At this time, the passive flexible guide pin is driven by the drive motor to retreat in the Z direction. As the supply-side docking substrate advances in the Z direction, the baffle contacts the spherical top column on the supply-side docking substrate. In the subsequent automatic docking process, the supply-side docking substrate can follow in five degrees of freedom in space except for the linear motion in the Z direction, so that it can passively adapt to the tiny gap between the supply-side docking substrate pin shaft and the receiving-side docking substrate pin hole, avoiding the generation of tangential force when the pin shaft and pin hole are docked. At the same time, it can also allow the terminal connector module to passively adapt to the tiny posture deviation between the modules during docking, so that the modules can be docked smoothly without being damaged, realizing the passive flexible docking of the automatic docking device.

[0119] When the device needs to be disconnected, the automatic docking system first controls the supply-side actuator to move backward, while monitoring the value of the position sensor installed on the supply-side docking base plate. When the rated value is reached, that is, the pin shaft has not completely disengaged from the pin hole, the automatic docking system controls the passive flexible docking mechanism to move, and the motor controls the passive flexible guide pin to move forward, causing it to fit tightly with the passive flexible guide hole, and fix the supply-side docking base plate and the supply-side box. Then the automatic docking system controls the actuator to quickly retreat to a safe disengagement position.

Claims

1. A six-degree-of-freedom, compliant, adaptive, integrated, large-scale connector automatic docking method based on vision and force feedback, characterized by: The automatic docking device includes a receiving end and several sets of supply ends; The receiving end comprises a receiving end frame, a receiving end docking substrate (1) and an active flexible docking mechanism (2); the receiving end frame is a rectangular frame with two open ends and a hollow interior; a base is provided below the receiving end frame; the receiving end frame is fixedly mounted on the ground via the base; the open side of the receiving end frame facing the supply end is sealed with the receiving end docking substrate (1); a receiving end guide pin hole (101) and a camera avoidance hole (102) are provided on the board surface of the receiving end docking substrate (1); The active flexible docking mechanism (2) is arranged in the inner cavity of the receiving end frame; the active flexible docking mechanism (2) includes 6 drive modules; each drive module includes a ball joint I (201), a connecting rod (202), a ball joint II (203) and a linear drive module; the linear drive modules are arranged on the four side walls of the receiving end frame; wherein, the upper wall of the receiving end frame is arranged with 2 linear drive modules, the lower wall is arranged with 2 linear drive modules, the left wall is arranged with 1 linear drive module, and the right wall is arranged with 1 linear drive module. Linear drive module; each linear drive module has a guide rail slider; the ball joint end surface mounting surface of the ball joint I (201) is fixedly connected to the guide rail slider; the ball joint mounting surface of the ball joint II (203) is fixed on the receiving end docking base plate (1); the connecting rod (202) is arranged between the corresponding ball joint I (201) and ball joint II (203); the ends of the connecting rod (202) are fixedly connected to the rotatable ends of the ball joint I (201) and ball joint II (203) respectively; 6 drive modules constitute a 6-pss mechanism; The supply end comprises a supply end frame, a supply end base, a docking substrate (3), a passive flexible docking mechanism (4) and a supply end actuator (5); the supply end frame is a rectangular frame with two open ends and a hollow interior; the supply end docking substrate (3) is arranged at an open portion of the supply end frame facing the receiving end; the passive flexible docking mechanism (4) is arranged in the inner cavity of the supply end frame; a supply end guide pin (301) and a visual camera (6) are provided on a side of the supply end docking substrate (3) facing the receiving end, and an adaptive plate (302) is provided on the side facing away from the receiving end; a three-dimensional force sensor is arranged at the rear end of the supply end guide pin (301); two passive flexible guide holes (3021) and a spherical top column (3022) are provided on the adaptive plate (302); the spherical top column (3022) is arranged between the two passive flexible guide holes (3021); The passive compliant docking mechanism (4) comprises a linear drive mechanism, a base and a passive compliant guide pin (404); the base is fixedly connected to the supply end frame; two return-to-zero guide rails (407) are arranged on the base; a slider (405) is slidably provided on the return-to-zero guide rail (407); the passive compliant guide pin (404) is fixedly connected to the slider (405); the linear drive mechanism can drive the slider (405) to move along the return-to-zero guide rail (407); The supply-end base is installed on the ground; the supply-end actuator (5) is placed on the supply-end base; the supply-end actuator (5) includes a linear drive actuator (502) and a receiving plate (501); the linear drive actuator (502) can drive the receiving plate (501) to move; the supply-end docking substrate (3) and the supply-end frame are both placed on the receiving plate (501); the linear drive actuator (502) drives the supply-end docking substrate (3) to move toward the receiving-end docking substrate (1); During operation, there is a posture deviation between the supply-end device and the receiving-end device; the automatic docking system recognizes the current posture through a visual camera (6) and a three-dimensional force sensor, and then controls the active flexible docking mechanism (2), successively performing a rough posture adjustment based on visual feedback and a precise posture adjustment based on force feedback; during the posture adjustment process, the supply-end actuator (5) drives the supply-end docking substrate (3) to move toward the receiving end, and finally completes the automatic docking of the supply-end docking substrate (3) and the receiving-end docking substrate (1); at the initial moment of docking, the passive flexible guide pin (404) is tightly embedded in the passive flexible guide hole (3021); the supply-end docking substrate (3) is in a zero-freedom fixed constraint state; after entering the automatic docking, the supply-end actuator (5) drives the supply-end docking substrate (3) to The receiving end docking substrate (1) moves in the direction of the receiving end docking substrate (1); when the supply end guide pin shaft (301) moves to the set length and enters the receiving end guide pin hole (101), the linear drive mechanism of the passive flexible docking mechanism (4) drives the passive flexible guide pin (404) to retreat to zero, so that the adaptive plate (302) is in a six-degree-of-freedom state, thereby making the supply end docking substrate (3) in a free docking state; the coupled movement of the supply end actuator (5) and the passive flexible docking mechanism (4) eliminates the posture deviation of the supply end docking substrate (3) and the receiving end docking substrate (1); by controlling the different positions of the guide rail sliders of the six drive modules in the linear direction, the posture of the receiving end docking substrate (1) in space is controlled, and finally the flexible active docking of the supply end docking substrate (3) and the receiving end docking substrate (1) of the active flexible docking mechanism is achieved.

2. The automatic docking method for large-scale connectors with six degrees of freedom and compliant self-adaptation based on vision and force feedback according to claim 1, characterized in that: The supply-end docking substrate (3) is provided with an air path connector (307), a high-current connector (303), an optical fiber module (304), a low-current module (305) and a network communication module (306) on its surface.

3. The automatic docking method for large-scale connectors with six degrees of freedom and compliant self-adaptation based on vision and force feedback according to claim 2, characterized in that: The receiving end docking substrate (1) is provided with a receiving end circuit connector, a receiving end gas path connector and a receiving end signal path connector corresponding to the supply end docking substrate (3) on the board surface.

4. The automatic docking method for large-scale connectors with six degrees of freedom and compliant self-adaptation based on vision and force feedback according to claim 1, characterized in that: A baffle (307) is also provided; the baffle (307) is arranged on the periphery of the adaptive plate (302); and a hollow portion is provided on the baffle (307) for the passive compliant guide pin (404) to pass through.

5. The automatic docking method of large-scale connectors with six degrees of freedom and compliant self-adaptation based on vision and force feedback according to claim 1, characterized in that: The docking method specifically includes the following steps: 1) Performing rough posture adjustment based on visual feedback; after the receiving end reaches the initial docking position, the automatic docking system starts working; performing preliminary posture recognition through the image captured by the industrial camera, and calculating the relative posture deviation between the supply end docking substrate (3) and the receiving end docking substrate (1) in the current posture; the active flexible docking mechanism (2) performs preliminary posture adjustment based on vision on the docking substrate; after the active flexible docking mechanism (2) is adjusted, the supply end actuator (5) is activated to drive the supply end forward; 2) Repeat step 1) until the industrial camera's field of view disappears; 3) Precise posture adjustment based on force feedback; 4) After rough and fine adjustments of the posture, the pin shaft at the supply end smoothly enters the pin hole at the receiving end, and the active and flexible docking mechanism (2) completes the assembly of the pin shaft into the pin hole; 5) The passive compliant guide pin is driven by the driving motor to retreat in the Z direction. As the supply-end docking substrate (3) moves forward in the Z direction, the baffle contacts the spherical top column on the supply-end docking substrate (3). In the subsequent automatic docking process, the supply-end docking substrate (3) can follow the five degrees of freedom in space except for the linear motion in the Z direction.

6. The automatic docking method for large-scale connectors with six degrees of freedom and compliant self-adaptation based on vision and force feedback according to claim 5, characterized in that: In step 1), the captured image is processed, and the characteristic circle is identified based on the captured images of the two pin holes and the coordinates of the points of the characteristic circle are calculated. Finally, the posture is solved and the target posture obtained is transmitted to the automatic docking control system. The system solves the inverse solution of the position of each slider of the active compliant docking mechanism (2) based on the target posture. The inverse solution formula of the kinematics of the mechanism is as follows: Where, l i is the connecting rod length, z Bi is the z coordinate of the spherical joint Bi in the global coordinate system; The automatic docking system then controls the corresponding linear drive module to move to the specified position, completing the initial posture adjustment of the six-degree-of-freedom active compliant docking device in the visual feedback stage.

7. The automatic docking method of large-scale connectors with six degrees of freedom and compliant self-adaptation based on vision and force feedback according to claim 5, characterized in that: Step 3) specifically includes the following sub-steps: 3.1) Analyze the force information measured by the two 3D force sensors. Establish the sensor coordinates O-XYZ at the center of the two 3D force sensors, and the measured force coordinates O′-X′Y′Z′ at the front ends of the two pins. These coordinates are parallel and share the same Z axis. Let F be the force acting on the force sensor, h be the distance between the two coordinate systems (i.e., the length of the pin), and d be the distance between pins 1 and 2 (i.e., the distance between 3D force sensors 1 and 2). The following mechanical model can be established. 3.2) identifying the relative position relationship between the supply-end docking substrate (3) and the receiving-end docking substrate (1) in the current chamfer contact state; 3.3) identifying the relative position relationship between the supply-end docking substrate (3) and the receiving-end docking substrate (1) in the current shaft-hole in-hole contact state; 3.4) During the execution of steps 3.2) and 3.3), the active compliant docking mechanism (2) is subjected to posture adjustment and position adjustment.

8. The automatic docking method for large-scale connectors with six degrees of freedom and compliant self-adaptation based on vision and force feedback according to claim 5, characterized in that: When the device needs to be disconnected, the automatic docking system first controls the supply-side actuator (5) to move backward, while monitoring the value of the position sensor installed on the supply-side docking base plate (3). When the rated value is reached, that is, the pin shaft has not completely disengaged from the pin hole, the automatic docking system controls the passive flexible docking mechanism (4) to move, and the motor controls the passive flexible guide pin to move forward, so that it fits tightly with the passive flexible guide hole, and the supply-side docking base plate (3) is fixed to the supply-side box. Then the automatic docking system controls the actuator to quickly retreat to a safe disengagement position.

Images