Cabin section pose docking method based on vision and force control

Abstract

The invention discloses a cabin posture docking method based on vision and force control, which comprises the following steps: performing vision measurement on poses of two to-be-docked cabins to obtain a vision measurement result; preliminarily adjusting the poses of the two to-be-butted cabin sections according to a visual measurement result, and adjusting the two to-be-butted cabin sections to a pre-butted position; a motor current signal in a posture adjusting mechanism is obtained, and contact force information of the cabin section butt joint face is obtained; and based on the contact force information of the butt joint surfaces of the cabin sections, precise butt joint of the cabin sections is realized by adopting a force compliance control method. According to the method, the safety of the butt joint process is ensured based on the current feedback information, and device damage caused by too large cabin section contact force can be effectively prevented; the method has the advantages of high efficiency, low cost, high adaptability, high precision, high safety coefficient and the like.

Description

Technical Field

[0001] The present invention mainly relates to the technical field of spacecraft, and particularly relates to a method for docking the position and attitude of a cabin section based on vision and force control. Background Art

[0002] The docking of cabin sections of spacecraft such as airplanes and missiles is a key link in their processing and manufacturing. The quality of the cabin section docking determines the quality of its assembly, and the assembly quality is closely related to the overall performance of the product. With the continuous development of the national defense industry, cabin section products show the characteristics of small batch and many types, and the diversification of product appearance puts forward higher requirements for the automatic docking system.

[0003] The existing precise docking methods for cabin sections mainly obtain the pose difference between the cabin sections to be docked through high-precision pose measurement methods, and then calculate the motion instructions of the attitude adjustment mechanism based on the measurement results, so as to achieve the precise docking of the cabin sections. Such docking methods have high requirements for the accuracy of the measurement results, and the docking effect is mainly determined by the measurement accuracy and the motion control accuracy of the attitude adjustment mechanism. The existing high-precision pose measurement methods mainly use sensors such as high-precision laser displacement sensors and laser scanners, which have problems such as high cost and poor adaptability. Moreover, such measurement methods cannot realize the real-time measurement of the pose of the cabin sections to be docked during the docking process, so the docking accuracy of the cabin sections and the safety of the docking device cannot be guaranteed when the system is interfered. Some methods ensure the safety of the docking process by installing six-dimensional force sensors to obtain the force during the docking process, but high-precision six-dimensional force sensors will greatly increase the cost of the docking system.

[0004] That is, the existing cabin section pose measurement technologies mostly perform pose measurement based on a certain type of sensor. For example, vision measurement based on cameras and laser measurement based on laser displacement sensors. However, it is difficult to achieve high-precision measurement in vision measurement, and there is a problem of small measurement range in laser measurement, resulting in the inability of the existing measurement methods to balance the requirements of universality and high precision. On the other hand, the measurement results obtained by a single measurement method cannot be re-verified before the cabin section docking, and the safety and reliability of the docking cannot be guaranteed. Some solutions use high-precision six-dimensional force sensors to monitor the contact force during the docking process, but this greatly increases the cost of the docking system.

[0005] In summary, the existing cabin section pose measurement technologies have the following deficiencies:

[0006] (1) The existing precise docking methods for cabin sections rely too much on high-precision pose measurement results

[0007] The realization of traditional precise docking of cabin segments requires calculating the pose difference of the cabin segments to be docked based on the results of high-precision pose measurement, and then achieving the precise docking of the cabin segments through the high-precision position control of the attitude adjustment mechanism. This docking method highly depends on the accuracy of the pose measurement method. However, the existing high-precision measurement methods have high requirements for the initial position of the cabin segments, with limited application scope, while the measurement methods with strong adaptability cannot achieve high measurement accuracy. Therefore, the traditional precise docking method of cabin segments cannot be widely promoted and applied.

[0008] (2) The existing precise docking methods of cabin segments cannot achieve reliable and real-time error feedback

[0009] The existing precise docking methods of cabin segments are generally realized based on the pose difference of the cabin segments to be docked obtained by the measurement method. First, the pose difference of the cabin segments to be docked is measured according to the initial pose of the cabin segments, and then the attitude adjustment mechanism of the cabin segments is driven to achieve the docking of the cabin segments. Due to the limitations of the high-precision pose measurement method, secondary measurement is generally not carried out during the docking process to ensure the safety and reliability of precise docking. At the same time, it is also difficult to achieve real-time pose feedback of the docking surface of the cabin segments.

[0010] The invention patent with the patent application number CN202110005821.4 and the name of "A Cabin Segment Pose Measurement and Alignment System, Control Method and Application" discloses a cabin segment pose measurement and alignment system, control method and application. The point cloud information of the cabin segment surface is obtained by scanning the surface of the cabin segment through a linear scanning method, and the pose information of the cabin segment is obtained by using a comprehensive method of axis fitting and generatrix fitting. According to the introduction in the patent specification, this method can achieve non-contact, target-free, large-range, automated and robust pose measurement. However, this method has high requirements for the surface accuracy of the cabin segment and the motion accuracy of the scanning sensor.

[0011] The invention patent with the patent application number CN202211242882.3 and the name of "An Accurate Attitude Adjustment and Docking Device and Method for an Aerospace Ramjet Engine Cabin Segment" discloses an accurate attitude adjustment and docking device and method for an aerospace ramjet engine cabin segment. The docking device consists of a six-degree-of-freedom attitude adjustment and docking platform, a pose measurement system, an attitude adjustment and docking control system and an industrial control computer. The position difference between the initial state and the target state of the target ball is measured by a laser tracker, and the motion command of the six-degree-of-freedom attitude adjustment and docking platform is obtained through calculation to achieve the docking of the cabin segment. This method can quickly achieve the docking of the cabin segment in a specific scenario, but it is difficult to achieve high-precision docking of the cabin segment, and the anti-interference ability is poor.

[0012] The invention patent with the patent application number CN117842397A and the name "A Visual Positioning Method for a High-Precision Cabin Section Docking System" discloses a visual positioning method for a high-precision cabin section docking system. By using a 3D camera, a digital model of the fixed cabin section is established, and then the axis direction and pre-docking pose of the docking cabin section are calculated. During the docking process, a six-axis force sensor is used to monitor the pressure and torsion during the docking process, and at the same time, a force control system is used to ensure the safety of the system. This method can achieve the accuracy and safety of the spacecraft docking process, but it requires the establishment of a cabin section digital model, with low efficiency, and the use of a six-axis force sensor greatly increases the cost of the docking system. Summary of the Invention

[0013] Aiming at the technical problems existing in the prior art, the present invention provides a cabin section pose docking method based on vision and force control with low cost and high docking accuracy.

[0014] To solve the above technical problems, the technical solution proposed by the present invention is as follows:

[0015] A cabin section pose docking method based on vision and force control, including the steps of:

[0016] Visually measure the poses of two cabin sections to be docked to obtain visual measurement results;

[0017] Preliminarily adjust the poses of the two cabin sections to be docked according to the visual measurement results to adjust to the pre-docking position;

[0018] Obtain the motor current signal in the attitude adjustment mechanism to obtain the contact force information of the cabin section docking surface;

[0019] Based on the contact force information of the cabin section docking surface, adopt a force compliance control method to achieve precise docking of the cabin sections.

[0020] Preferably, the specific process of preliminarily adjusting the poses of the two cabin sections to be docked according to the visual measurement results is as follows:

[0021] Calculate the relative position relationship between the cameras through the camera calibration process;

[0022] Based on the feature circles with known shapes and sizes, solve the three-dimensional coordinates of the centers of the feature circles in their camera coordinate systems; where each feature map is the flange hole of the cabin section measured by each camera;

[0023] Convert the centers of the feature circles to a unified coordinate system according to the relative position relationship between the cameras to obtain the converted center coordinates;

[0024] Based on the unified coordinate system, complete the fitting of the spatial circle where the converted center coordinates are located, and then complete the solution of the normal vector of the spatial circle passing through the center of the circle, and calculate and solve the cabin section pose; where the normal vector of the center of the circle is the axis of the cabin section.

[0025] Preferably, the specific process of the force compliance control method is as follows:

[0026] According to the motor current data, and then based on the mapping relationship between the current and the load torque, the actual load torque F is obtained e , and then it is combined with the desired torque F r After synthesis, the difference is input to the admittance controller, and then the position correction amount x is obtained e ;

[0027] In the position loop, the desired position x r , the actual position x and the position correction amount x e After the three are synthesized, they are input to the position controller, so as to realize the compliance control of the cabin section attitude adjustment mechanism, and then complete the precise docking of the cabin section.

[0028] Preferably, the output of the position controller is sequentially passed through the speed loop and the current loop, and then a control signal is output to the cabin section attitude adjustment mechanism to realize compliance control.

[0029] The present invention also discloses a cabin section position and attitude docking device based on vision and force control, including a control unit, a plurality of vision measurement units and a current measurement unit; a plurality of the vision measurement units are located on the docking flange side of the cabin section to be docked; the current measurement unit is located on the motor; the control unit is connected to the plurality of vision measurement units and the current measurement unit, and is used to perform position and attitude adjustment of the cabin section to be docked according to the vision measurement result and the current measurement result.

[0030] Preferably, the vision measurement unit is a camera.

[0031] Preferably, the current measurement unit is a current sensor.

[0032] The present invention further discloses a computer program product, including a computer program, and when the computer program is run by a processor, it executes the steps of the method described above.

[0033] The present invention also discloses a computer-readable storage medium, on which a computer program is stored, and when the computer program is run by a processor, it executes the steps of the method described above.

[0034] The present invention further discloses a cabin section position and attitude docking system based on vision and force control, including a memory and a processor connected to each other, a computer program is stored on the memory, and when the computer program is run by the processor, it executes the steps of the method described above.

[0035] Compared with the prior art, the advantages of the present invention are as follows:

[0036] The present invention realizes precise docking of cabin sections with strong adaptability and high safety factor based on visual detection and motor current feedback. The proposed method adopts a rough-fine composite docking strategy. Among them, the preliminary docking based on the visual measurement results can achieve the preliminary contact of the docking surface of the cabin section under general working conditions. The contact force is estimated by the mapping between the motor current and the contact force, and then the compliant control docking strategy based on the contact force is used to feedback the state of the docking surface in real time to ensure the safety and reliability of the precise docking process.

[0037] The docking method of the present invention relies on a camera and a posture adjustment mechanism to realize. The machining surface features of the docking end of the cabin section are detected by a multi-camera, and the pose of the cabin section is measured based on the prior knowledge of the feature size and the multi-view geometry knowledge. The measurement information is fed back to the posture adjustment mechanism to realize the preliminary docking of the cabin section; after the preliminary docking is completed, the docking surface of the cabin section achieves preliminary contact, and the contact force information of the docking surface of the cabin section is estimated based on the mapping relationship between the motor current and the load torque, and the high-precision and safe docking of the cabin section is realized through a force control algorithm.

[0038] The method of the present invention ensures the safety of the docking process based on the current feedback information, and can effectively prevent the device damage caused by excessive contact force of the cabin section; the above method has the advantages of high efficiency, low cost, strong adaptability, high precision, high safety factor, etc. Description of the Drawings

[0039] Figure 1 It is a flowchart of the cabin section pose docking method of the present invention in an embodiment.

[0040] Figure 2 It is a flowchart of using the visual measurement results for cabin docking in the present invention.

[0041] Figure 3 It is a structural diagram of the cabin section pose docking device of the present invention in an embodiment.

[0042] Figure 4 It is a control block diagram of the force compliant control strategy of the present invention in an embodiment.

[0043] Figure 5 It is a comparison experimental diagram of the present invention with and without an impedance controller. Detailed Embodiment

[0044] The present invention will be further described below in conjunction with the specification drawings and specific embodiments.

[0045] As Figure 1 shown, the cabin section pose docking method based on vision and force control provided by the embodiment of the present invention includes the steps:

[0046] Visually measure the poses of two cabin sections to be docked to obtain visual measurement results;

[0047] Based on the visual measurement results, preliminarily adjust the poses of the two docking cabin sections to the pre-docking position.

[0048] Obtain the motor current signal in the attitude adjustment mechanism to obtain the contact force information of the docking surface of the cabin section.

[0049] Based on the contact force information of the docking surface of the cabin section, adopt the force compliance control method to achieve the precise docking of the cabin section.

[0050] The present invention realizes the precise docking of the cabin section with strong adaptability and high safety factor based on visual detection and motor current feedback. The proposed method adopts a rough and fine composite docking strategy. Among them, the preliminary docking based on the visual measurement results can realize the preliminary contact of the docking surface of the cabin section under general working conditions. The contact force is estimated by the mapping between the motor current and the contact force, and then the compliance control docking strategy based on the contact force is used to feedback the state of the docking surface in real time to ensure the safety and reliability of the precise docking process.

[0051] As Figure 3 shown, the embodiment of the present invention also provides a cabin section attitude docking device based on vision and force control, including a control unit, a visual measurement unit, a current measurement unit, and an attitude adjustment unit; among them, the attitude adjustment system adopts a standardized six-degree-of-freedom attitude adjustment mechanism (which belongs to a conventional structure and will not be elaborated here).

[0052] Multiple visual measurement units (such as cameras) are located on the docking flange side of the docking cabin section; the current measurement unit (such as a current sensor, not shown in the figure) is located on the motor to detect the motor current; the control unit is connected to multiple visual measurement units and current measurement units, and is used to adjust the poses of the docking cabin sections according to the visual measurement results and current measurement results, specifically as follows:

[0053] First, use the camera to preliminarily measure the poses of the two docking cabin sections, and feedback the measurement results to the attitude adjustment mechanism. The attitude adjustment mechanism adjusts the poses of the two cabin sections according to the measurement results (in this process, one of the cabin sections can be fixed and the pose of the other cabin section can be adjusted to reduce the calculation amount of the attitude adjustment system). After this process, the two cabin sections are in the preliminary docking state, that is, there is a contact force between the docking surfaces.

[0054] Then, estimate the contact force information of the docking surface of the cabin section based on the mapping relationship between the motor current and the load torque, and adopt the force compliance control method (the docking method described above) to achieve the precise docking of the cabin section.

[0055] Specifically, multiple cameras with small fields of view and high resolutions are used and distributed circumferentially along the cabin section to achieve the acquisition of cabin section images from multiple perspectives. After the cameras are installed, the Zhang Zhengyou method or other methods are used for calibration to obtain the internal and external parameters of the cameras. The collected images are processed using an optimized ellipse detection algorithm to obtain the two-dimensional coordinate information of the center positions of the machining holes in the images. Geometric transformation is performed based on the external parameters obtained through calibration to transform the two-dimensional coordinate information in different camera coordinate systems to the world coordinate system, realizing the stitching of image feature information from different perspectives. Combining the structural information of the cabin section with the distribution of the machining hole center coordinates to fit a spatial circle, and calculating the pose of the cabin section based on the spatial circle information. The specific process is as Figure 2 shown:

[0056] 1) Install cameras A, B, and C, and calculate the relative position relationship (R, T matrices) between cameras A, B, and C through the camera calibration process;

[0057] 2) Based on the feature circles a, b, and c with known shapes and sizes, solve the three-dimensional coordinates of the centers a, b, and c in their camera coordinate systems;

[0058] 3) Convert the centers a, b, and c to a unified coordinate system according to the relative position relationship between cameras A, B, and C, denoted as a', b', and c';

[0059] 4) Based on the unified coordinate system, complete the fitting of the spatial circle where a', b', and c' are located, and then complete the solution of the normal vector (cabin section axis) passing through the center of the spatial circle, and calculate the pose of the cabin section.

[0060] As Figure 4 shown, according to the docking requirements, position-based impedance control (admittance control) is adopted as the compliant control strategy, and the corresponding control system mainly consists of a current loop, a speed loop, a position loop, and an impedance control loop;

[0061] To ensure the accuracy of contact force estimation, a high-precision force sensor is used to calibrate the servo mechanism, collect sufficient information on current and load torque, and use an intelligent algorithm to realize the mapping between current and load torque;

[0062] In specific applications, the impedance control loop obtains the actual load torque F e based on the motor current data and according to the mapping relationship between current and load torque, and then combines it with the desired torque F r to obtain the difference and input it into the admittance controller, and then obtain the position correction amount x e ;

[0063] In the position loop, the desired position x r , the actual position x, and the position correction amount x eThe three are combined and input into the position controller. The output of the position controller passes through the speed loop and the current loop in sequence, and then outputs a control signal to the cabin attitude adjustment mechanism, thereby realizing the compliant control of the cabin attitude adjustment mechanism, and then completing the precise docking of the cabin section.

[0064] Figure 5 The figure shows a comparison experiment diagram with and without an impedance controller. Figure 5 The position change of a certain degree of freedom during the cabin attitude adjustment is shown. When the cabin is at 5°, it is subjected to an external resistance, starting from Figure 5 It can be seen from the figure that the cabin section with the impedance controller stops at 5°, while the cabin section without the impedance controller continues to move along the reference trajectory and collides with the outside. The experiment shows that using an impedance controller can effectively avoid collisions during the cabin docking process, and the docking pose can be corrected through the contact force, thereby achieving high-precision docking of the cabin section.

[0065] The docking method of the present invention is realized relying on a camera and an attitude adjustment mechanism. The processing surface features of the docking end of the cabin section are detected by a multi-view camera. Based on the prior knowledge of the feature size and the multi-view geometry knowledge, the pose of the cabin section is measured. The measurement information is fed back to the attitude adjustment mechanism to achieve the preliminary docking of the cabin section; after the preliminary docking is completed, the docking surfaces of the cabin section are in preliminary contact. Based on the mapping relationship between the motor current and the load torque, the contact force information of the docking surface of the cabin section is estimated, and the high-precision and safe docking of the cabin section is realized through a force control algorithm.

[0066] The method of the present invention ensures the safety of the docking process based on the current feedback information, and can effectively prevent device damage caused by excessive contact force of the cabin section; the above method has the advantages of high efficiency, low cost, strong adaptability, high precision, high safety factor, etc.

[0067] The present invention further discloses a computer program product, including a computer program, and the computer program executes the steps of the above method when being run by a processor. The present invention also discloses a computer-readable storage medium, on which a computer program is stored, and the computer program executes the steps of the above method when being run by a processor. The present invention further discloses a cabin attitude docking system based on vision and force control, including a memory and a processor connected to each other. A computer program is stored on the memory, and the computer program executes the steps of the above method when being run by the processor. The products, media and systems of the present invention all correspond to the above method and have the same advantages as those described in the above method.

[0068] All or part of the processes in the above-described embodiment methods of the present invention can also be completed by hardware related to computer program instructions. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by a processor, the steps of the above method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in the form of source code, object code, executable file, or some intermediate form, etc. The computer-readable storage medium includes: any entity or device capable of carrying computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electrical carrier signal, telecommunication signal, and software distribution medium, etc. The memory is used to store computer programs and / or modules. The processor realizes various functions by running or executing the computer programs and / or modules stored in the memory, and by calling the data stored in the memory. The memory can include high-speed random access memory, and can also include non-volatile memory, such as hard disk, memory, plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, at least one magnetic disk storage device, flash device, or other volatile solid-state storage devices, etc.

[0069] The above is only the preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions falling within the concept of the present invention belong to the protection scope of the present invention. It should be noted that for those of ordinary skill in the art in the technical field, several improvements and refinements made without departing from the principle of the present invention should be regarded as within the protection scope of the present invention.

Claims

1. A method for docking the cabin position and attitude based on vision and force control, characterized in that Including the steps: Visually measure the poses of two segments to be docked to obtain the visual measurement results; Preliminarily adjust the poses of the two segments to be docked according to the visual measurement results to adjust them to the pre-docking positions; Obtain the motor current signals in the attitude adjustment mechanism to obtain the contact force information of the segment docking surface; Based on the contact force information of the segment docking surface, adopt a force compliance control method to achieve precise docking of the segments.

2. The method for docking the cabin position and attitude based on vision and force control according to claim 1, wherein The specific process of preliminarily adjusting the poses of the two segments to be docked according to the visual measurement results is as follows: Calculate the relative position relationship between the cameras through the camera calibration process; Based on the feature circles with known shapes and sizes, solve the three-dimensional coordinates of the centers of the respective feature circles in their camera coordinate systems; where each feature map is the flange hole of the segment measured by each camera; Convert the centers of the respective feature circles to a unified coordinate system according to the relative position relationship between the cameras to obtain the converted center coordinates; Complete the fitting of the space circle where the converted center coordinates are located based on the unified coordinate system, and then complete the solution of the normal vector of the space circle passing through the center of the circle to calculate and obtain the segment pose; where the normal vector of the center of the circle is the segment axis.

3. The method for docking the cabin position and attitude based on vision and force control according to claim 2, wherein, Among them, the specific process of the force compliance control method is as follows: According to the motor current data, the actual load torque F is obtained based on the mapping relationship between the current and the load torque e , and then it is combined with the desired torque F r . After synthesis, the difference is input into the admittance controller, and then the position correction amount x is obtained e ; In the position loop, the desired position x r , the actual position x, and the position correction amount x e are combined and input into the position controller, thereby realizing the compliant control of the cabin attitude adjustment mechanism and completing the precise docking of the cabin section.

4. The method for docking the cabin position and attitude based on vision and force control according to claim 3, wherein The output of the position controller sequentially passes through the speed loop and the current loop, and then outputs a control signal to the segment attitude adjustment mechanism to achieve compliance control.

5. A cabin position and attitude docking device based on vision and force control, characterized in that, Including a control unit, a plurality of visual measurement units, and a current measurement unit; a plurality of the visual measurement units are located on the docking flange side of the segment to be docked; the current measurement unit is located on the motor; the control unit is connected to the plurality of visual measurement units and the current measurement unit, and is used to adjust the pose of the segment to be docked according to the visual measurement results and the current measurement results.

6. The pose docking device for the cabin based on vision and force control according to claim 5, wherein The visual measurement unit is a camera.

7. The pose docking device for the cabin based on vision and force control according to claim 5, characterized in that The current measurement unit is a current sensor.

8. A computer program product comprising a computer program, characterized in that, When the computer program is run by a processor, it executes the steps of the method described in any one of claims 1-4.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is run by a processor, it executes the steps of the method described in any one of claims 1-4.

10. A cabin position and attitude docking system based on vision and force control, comprising a memory and a processor connected to each other, wherein a computer program is stored on the memory, and is characterized in that, When the computer program is run by a processor, it executes the steps of the method described in any one of claims 1-4.

Images