Space cooperative target and relative position measurement method based on TOF system

Abstract

The present invention discloses a space cooperative target based on a TOF system and a relative position and posture measurement method thereof. The method comprises: designing a three-dimensional cooperative target, developing a camera based on a TOF system, the TOF camera continuously photographs the cooperative target, obtains a depth image and a grayscale image, and then completes identification matching and tracking, and performs calculation with target information pre-stored in a memory to obtain relative position and posture information. The present invention continuously photographs the cooperative target through a TOF camera, obtains a depth image and a grayscale image, completes identification matching and tracking, and performs calculation with target information pre-stored in a memory to obtain relative position and posture information, has low requirements on target tracking and re-capture capabilities, and has a simple algorithm and strong real-time performance.

Description

Technical Field

[0001] The present invention relates to the technical field of space phase attitude measurement, and in particular to a space cooperative target based on a TOF system and a relative position and attitude measurement method thereof. Background Art

[0002] Relative posture measurement technology is widely used in various space missions. With the increasing number of space rendezvous and docking and space manipulation missions, the demand for space relative posture measurement technology is becoming increasingly prominent. Space relative posture measurement technology refers to the measurement of the relative posture and relative position between the target coordinate system and the measurement coordinate system through a certain optical measurement system, and then inputting the information into the GNC system for use in the subsequent posture control of spacecraft.

[0003] Currently, commonly used methods for measuring relative spatial posture include binocular cameras, laser rangefinders, and TOF cameras. Laser rangefinders can provide target distance information and have the characteristics of low power consumption, low price, simple structure, and insensitivity to light, but laser rangefinders need to have strong target tracking and recapture capabilities; binocular stereo vision has the characteristics of high resolution, low power consumption, and low price, but it still has problems such as short ranging range, complex calibration, complex algorithms, poor real-time performance, and poor adaptability to lighting conditions. Summary of the invention

[0004] The purpose of the present invention is to provide a space cooperative target and relative posture measurement method based on TOF system, wherein the method aims to solve the problems that the traditional method has high requirements on target tracking and recapture capability, complex algorithm and poor real-time performance when performing space relative posture measurement.

[0005] To achieve the above object, on the one hand, the present invention provides a space cooperation target based on the TOF system, comprising:

[0006] A corner cube prism;

[0007] Seven far-field markers, the corner cube prism and the seven far-field markers are used to identify and measure the target position at different distance segments,

[0008] Based on four target recognition principles, the positions and serial numbers of the seven far-field markers are identified.

[0009] Preferably, the seven far-field markers are: a first target, a second target, a third target, a fourth target, a fifth target, a sixth target and a seventh target.

[0010] Preferably, the four target recognition principles include:

[0011] The first target, the second target and the third target meet the co-linear requirement;

[0012] The first target, the fifth target and the sixth target meet the co-linear requirement;

[0013] The intersection of the line connecting the fourth target and the fifth target and the line connecting the first target and the third target is a first intersection C1, the intersection of the line connecting the fourth target and the sixth target and the line connecting the first target and the third target is a second intersection C2, the first target, the second target, the first intersection C1 and the third target meet the cross ratio condition, and the first target, the second target, the second intersection C2 and the third target meet the cross ratio condition;

[0014] The intersection of the line connecting the second target and the fourth target and the line connecting the first target and the sixth target is the third intersection C3, and the first target, the third intersection C3, the fifth target and the sixth target meet the intersection ratio condition.

[0015] On the other hand, the present invention also provides a method for measuring relative position and posture of space cooperative targets based on the TOF system, which uses the above-mentioned cooperative targets to perform relative position and posture measurement.

[0016] Step S1: photographing the cooperative target using a TOF camera to obtain a depth image and a grayscale image;

[0017] Step S2: Processing the grayscale image to extract the centroid of the cooperative target;

[0018] Step S3: Select the pixel with the highest grayscale in the grayscale image, extract the depth information corresponding to the pixel, and obtain the line of sight distance of the pixel;

[0019] Step S4: determining the size of the sight distance and the posture solution distance range to obtain a determination result, and searching and matching the cooperative target according to the determination result to obtain a recognition result;

[0020] Step S5: performing posture calculation of the current frame according to the recognition result and the posture calculation result of the previous frame.

[0021] Preferably, in step S3, the depth information corresponding to the pixel point is extracted to obtain the line of sight distance of the pixel point, which is specifically: the average of the distance values ​​of the four pixels adjacent to the pixel point is defined as the line of sight distance, and the line of sight angle of the target position is calculated according to the coordinates of the pixel point.

[0022] Preferably, in step S4, the size of the sight distance and the posture solution distance range is determined, which is specifically: judging whether the sight distance is within the posture solution distance range, if it is within the posture solution range, performing posture solution; if it is not within the posture solution range, returning to step S1 to calculate the next frame of image data.

[0023] Preferably, in step S4, the cooperative targets are searched and matched to obtain identification results, which are specifically: the cooperative targets are identified in sequence through the four target identification principles.

[0024] Preferably, in step S5, the posture solution of the current frame is performed according to the recognition result and the posture solution result of the previous frame, which is specifically: if the recognition result is successful, the posture solution of the current frame is performed; if the recognition result is successful and the posture solution result of the previous frame is completed, the target is tracked according to the posture calculated in the previous frame, the coordinate position of the target is back-projected, the center of mass of the cooperative target is obtained, and the posture calculation of the current frame is performed.

[0025] Preferably, the posture solution of the current frame specifically includes: solving the rotation matrix through iteration according to the collinear equation and the indirect adjustment principle.

[0026] Compared with the prior art, the present invention has the following beneficial effects:

[0027] The present invention continuously shoots the cooperative target through the TOF camera, obtains the depth image and the grayscale image, completes the recognition matching and tracking, and solves with the target information pre-stored in the memory to obtain the relative position and posture information. The present invention has low requirements on the target tracking and re-capture capabilities, and the algorithm is simple and has strong real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the technical solution of the present invention, the following briefly introduces the drawings required for the description. Obviously, the drawings described below are an embodiment of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative work:

[0029] Figure 1 A design diagram of a space cooperation target based on a TOF system provided by an embodiment of the present invention;

[0030] Figure 2 A flowchart of a method for measuring relative posture of space cooperative targets based on a TOF system is provided for one embodiment of the present invention.

[0031] Explanation of the reference numerals: 101 - first target, 102 - second target, 103 - third target, 104 - fourth target, 105 - fifth target, 106 - sixth target, 107 - seventh target, 201 - corner cube prism, 301 - near-field marker. DETAILED DESCRIPTION

[0032] The following is combined with Figure 1-2 The present invention further describes in detail a space cooperative target based on the TOF system and a relative posture measurement method thereof according to the specific implementation mode. According to the following description, the advantages and features of the present invention will be clearer. It should be noted that the drawings are in a very simplified form and use non-precise proportions, which are only used to conveniently and clearly assist in explaining the purpose of the implementation mode of the present invention. In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, please refer to the drawings. It should be noted that the structure, proportion, size, etc. illustrated in the drawings of this specification are only used to match the content disclosed in the specification for people familiar with this technology to understand and read, and are not used to limit the limiting conditions for the implementation of the present invention, so they have no technical substantive significance. Any modification of the structure, change of the proportional relationship or adjustment of the size, without affecting the effect that the present invention can produce and the purpose that can be achieved, should still fall within the scope of the technical content disclosed by the present invention.

[0033] In view of the problems that traditional methods have high requirements on target tracking and re-capture capabilities, complex algorithms and poor real-time performance when performing spatial relative pose measurement.

[0034] On the one hand, this embodiment provides a method for measuring the relative posture of a space cooperative target based on a TOF system, including: a corner cube prism 201; seven far-field markers, the corner cube prism and the seven far-field markers are used to identify and measure the target position at different distances, respectively, based on four target recognition principles, the positions and serial numbers of the seven far-field markers are identified, and a near-field marker 301.

[0035] TOF cameras can directly obtain the depth and intensity information of the target without a scanning mechanism. They have the advantages of low power consumption, compact structure, good real-time performance and insensitivity to light. They are more suitable for close-range measurement of space targets.

[0036] like Figure 1 As shown, in this embodiment, the seven far-field markers are: a first target 101 , a second target 102 , a third target 103 , a fourth target 104 , a fifth target 105 , a sixth target 106 and a seventh target 107 .

[0037] The four target recognition principles include: 1. The first target 101, the second target 102 and the third target 103 meet the collinear requirement; 2. The first target 101, the fifth target 105 and the sixth target 106 meet the collinear requirement; 3. The intersection point of the line connecting the fourth target 104 and the fifth target 105 and the line connecting the first target 101 and the third target 103 is the first intersection point C1, and the intersection point of the line connecting the fourth target 104 and the sixth target 106 and the line connecting the first target 101 and the third target 103 is the second intersection point C2. Point C2, the first target 101, the second target 102, the first intersection C1 and the third target 103 meet the cross-ratio condition, and the first target 101, the second target 102, the second intersection C2 and the third target 103 meet the cross-ratio condition; Fourth, the intersection of the line connecting the second target 102 and the fourth target 104 and the line connecting the first target 101 and the sixth target 106 is the third intersection C3, then the first target 101, the third intersection C3, the fifth target 105 and the sixth target 106 meet the cross-ratio condition.

[0038] On the other hand, this embodiment also provides a method for measuring relative pose of a space cooperative target based on a TOF system, which uses the above-mentioned cooperative target to perform relative pose measurement, including the following steps:

[0039] Step S1: Use a TOF camera to photograph the cooperative target to obtain a depth image and a grayscale image.

[0040] Step S2: Process the grayscale image to extract the center of mass of the cooperative target.

[0041] Step S3: Select the pixel with the highest grayscale in the grayscale image, extract the depth information corresponding to the pixel, and obtain the line of sight distance of the pixel.

[0042] The extracting of the depth information corresponding to the pixel point to obtain the sight distance of the pixel point is specifically as follows: defining the average of the distance values ​​of the four pixels adjacent to the pixel point as the sight distance, and calculating the sight angle of the target location according to the coordinates (X0, Y0) of the pixel point.

[0043] Step S4: Determine the size of the sight distance and the posture solution distance range to obtain a determination result, and search and match the cooperative target according to the determination result to obtain a recognition result.

[0044] Determine the size of the sight distance and the attitude solution distance range, which is specifically: determine whether the sight distance is within the attitude solution distance range (the attitude solution distance range is 15m in this embodiment); if it is within the attitude solution range (that is, the sight distance is less than or equal to 15m), perform attitude solution; if it is not within the attitude solution range (that is, the sight distance is greater than 15m), return to step S1 to calculate the next frame of image data.

[0045] The cooperative targets are searched and matched to obtain identification results, which are specifically: the cooperative targets are identified in turn through the four target identification principles.

[0046] Step S5: performing posture calculation of the current frame according to the recognition result and the posture calculation result of the previous frame.

[0047] refer to Figure 2 It can be seen that after performing posture calculation for the current frame, the system returns to step S1 to perform posture calculation for the next frame.

[0048] According to the recognition result and the posture solution result of the previous frame, the posture solution of the current frame is performed, which is specifically: if the recognition result is successful, the posture solution of the current frame is performed; if the recognition result is successful and the posture solution result of the previous frame is completed, according to the posture calculated in the previous frame, target tracking is performed, the coordinate position of the target is back-projected, the center of mass of the cooperative target is obtained, and the posture calculation of the current frame is performed.

[0049] The posture solution of the current frame specifically includes: solving the rotation matrix through iteration according to the collinear equation and the indirect adjustment principle.

[0050] The specific implementation example of solving the posture is as follows: the target imaging effect of the detector based on the corner cone prism is a circular center light spot, and a weighted grayscale centroid algorithm is proposed to calculate the center of mass of the light spot, and then the pitch angle and yaw angle of the center of mass of the light spot are extracted.

[0051] Assume that the spot pixels are (x1, y1), (x2, y2), ... (x n ,y n ), then the expression of weighted grayscale centroid coordinates (x0, y0) is:

[0052]

[0053] Where: w i is the coordinate (x i ,y i )’s grayscale value;

[0054] The expression of the elevation angle β is:

[0055] β=(y0-Y0)×θ (2)

[0056] The expression of the azimuth angle α is:

[0057] α=(x0-X0)×θ (3)

[0058] Where (X0, Y0) is the coordinate of the detector principal point, and θ is the angular resolution of the detector.

[0059] Assume that the target grayscale extreme value coordinates of the corner cube prism are (x m ,y m ), take the average of the distance values ​​of the four adjacent pixels as the line of sight distance, and its expression is:

[0060]

[0061] In the formula is the pixel coordinate (x i ,y i ) distance value.

[0062] Attitude and position measurements are performed using far-field targets in the 1.5m to 30m range.

[0063] Reference Figure 1 As shown, by recognizing and solving the captured grayscale image, the recognition criterion is:

[0064] 1. The first target 101, the second target 102 and the third target 103 meet the collinear requirement; 2. The first target 101, the fifth target 105 and the sixth target 106 meet the collinear requirement; 3. The intersection of the line connecting the fourth target 104 and the fifth target 105 and the line connecting the first target 101 and the third target 103 is the first intersection C1, the intersection of the line connecting the fourth target 104 and the sixth target 106 and the line connecting the first target 101 and the third target 103 is the second intersection C2, and the One target 101, the second target 102, the first intersection C1 and the third target 103 meet the cross-ratio condition, and the first target 101, the second target 102, the second intersection C2 and the third target 103 meet the cross-ratio condition; Four, the intersection of the line connecting the second target 102 and the fourth target 104 and the line connecting the first target 101 and the sixth target 106 is the third intersection C3, then the first target 101, the third intersection C3, the fifth target 105 and the sixth target 106 meet the cross-ratio condition.

[0065] The extracted target points are cyclically identified and matched. If the above conditions are met, the identification is successful, otherwise it is unsuccessful.

[0066] The target tracking function module is divided into three parts. The first part back-projects the position of the large target (i.e., far-field mark) according to the previous frame pose. The second part uses the large target (i.e., far-field mark) to solve the pose if the number of large targets (i.e., far-field marks) in the field of view is greater than 5. The third part back-projects all targets and solves the pose. Back-project the target point according to the final iterative result and calculate the back-projection error. The error is used to determine whether the current pose is valid.

[0067] After completing target identification or tracking, the relative pose and position can be solved. This solution uses the collinear equation and the indirect adjustment principle to iteratively solve the rotation matrix.

[0068] According to the collinear condition equation in photogrammetry theory:

[0069]

[0070] Among them, (x, y) is the coordinate of the image point; (x0, y0, f) is the internal orientation element of the camera; (X, Y, Z) is the Earth-aided coordinate of the object point corresponding to the image point; (X S ,Y S ,Z S ) is the exterior orientation line element of the camera; a1, a2, a3, b1, b2, b3, c1, c2, c3 form the rotation matrix M, which can be expressed as:

[0071]

[0072] M is also the camera's exterior azimuth element The equivalent expression of .

[0073] The rotation matrix algorithm abandons the description of the matrix M as the exterior azimuth element The traditional approach is to directly use X S ,Y S ,Z S , a1, a2, a3, b1, b2, b3, c1, c2, c3 are unknown parameters, the image point coordinates (x, y) are observed values, the camera internal orientation elements and the ground coordinate points (X, Y, Z) are known values, and equation (6) is linearized to obtain the error equation:

[0074]

[0075] The error equation (7) above can be written in matrix form as:

[0076] V=Cδ X +1 (8)

[0077] in,

[0078] V=[v x vy ]T (9)

[0079] Measurements

[0080] Correction value δ X =[dX S dY S Z S da1 db1...dc3]T (11)

[0081] l=[l x l y ]T (12)

[0082] Since the matrix M is an orthogonal matrix, the above unknown quantity δ X The following six constraints are also implied, and the six constraints are shown in the following formula (13):

[0083]

[0084] Linearize the above equation to get

[0085] B X δ X +W=0 (14)

[0086] in

[0087]

[0088]

[0089] According to the above formula, two error equations can be listed for the 8 control points, so the original error equation of the overall adjustment is

[0090]

[0091] Where V and l are 16×1 matrices, and C is a 16×12 matrix. If the weight matrix of the observation value is P, the indirect adjustment model is used to solve it, and we get

[0092] Y=-N Y -1 W Y (16)

[0093] in,

[0094] Given the initial value of the coordinate position and the rotation matrix, the value is calculated according to equation (16) and iterated step by step until the correction number δ X Less than the tolerance.

[0095] To summarize, this embodiment uses a TOF camera to continuously shoot the cooperative target, obtain a depth image and a grayscale image, and then completes recognition, matching and tracking, and solves with the target information pre-stored in the memory to obtain the method of relative position and posture information. The method has low requirements on target tracking and re-capture capabilities, a simple algorithm and strong real-time performance, and solves the problems in the prior art of high requirements on target tracking and re-capture capabilities, complex algorithms and poor real-time performance when performing spatial relative posture measurement.

[0096] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0097] It should be noted that the devices and methods disclosed in the embodiments of this article can also be implemented in other ways. The device implementation described above is only schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of this article. In this regard, each box in the flowchart or block diagram can represent a part of a module, program or code, and the module, program segment or part of the code contains one or more executable instructions for implementing the specified logical function, and the module, program segment or part of the code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented by a dedicated hardware-based system for performing a specified function or action, or can be implemented by a combination of dedicated hardware and computer instructions.

[0098] Although the content of the present invention has been described in detail through the above preferred embodiments, it should be appreciated that the above description should not be considered as a limitation of the present invention. After reading the above content, it will be apparent to those skilled in the art that various modifications and substitutions of the present invention will occur. Therefore, the protection scope of the present invention should be limited by the appended claims.

Claims

1. A space cooperative target based on TOF system, characterized in that: include: A corner cube prism; Seven far-field markers, the corner cube prism and the seven far-field markers are used to identify and measure the target position at different distance segments, Based on the four target recognition principles, the positions and serial numbers of the seven far-field markers are identified. The seven far-field markers are: a first target, a second target, a third target, a fourth target, a fifth target, a sixth target and a seventh target, respectively. The four target identification principles include: The first target, the second target and the third target meet the co-linear requirement; The first target, the fifth target and the sixth target meet the co-linear requirement; The intersection of the line connecting the fourth target and the fifth target and the line connecting the first target and the third target is a first intersection C1, the intersection of the line connecting the fourth target and the sixth target and the line connecting the first target and the third target is a second intersection C2, the first target, the second target, the first intersection C1 and the third target meet the cross ratio condition, and the first target, the second target, the second intersection C2 and the third target meet the cross ratio condition; The intersection of the line connecting the second target and the fourth target and the line connecting the first target and the sixth target is the third intersection C3, and the first target, the third intersection C3, the fifth target and the sixth target meet the intersection ratio condition.

2. A method for measuring relative position and posture of space cooperative targets based on TOF system, characterized in that: It uses the cooperative target described in claim 1 to perform relative posture measurement. Step S1: photographing the cooperative target using a TOF camera to obtain a depth image and a grayscale image; Step S2: Processing the grayscale image to extract the centroid of the cooperative target; Step S3: Select the pixel with the highest grayscale in the grayscale image, extract the depth information corresponding to the pixel, and obtain the line of sight distance of the pixel; Step S4: Determine the size of the sight distance and the attitude solution distance range to obtain a determination result, which is specifically: determine whether the sight distance is within the attitude solution distance range, if it is within the attitude solution range, perform attitude solution; if it is not within the attitude solution range, return to step S1 to calculate the next frame of image data, According to the determination result, searching and matching the cooperation target to obtain an identification result; Step S5: performing posture calculation of the current frame according to the recognition result and the posture calculation result of the previous frame.

3. The method for measuring relative position and posture of space cooperative targets based on the TOF system as claimed in claim 2, characterized in that: In step S3, the depth information corresponding to the pixel point is extracted to obtain the line of sight distance of the pixel point, which is specifically: the average of the distance values ​​of the four pixels adjacent to the pixel point is defined as the line of sight distance, and the line of sight angle of the target position is calculated according to the coordinates of the pixel point.

4. The method for measuring relative position and posture of space cooperative targets based on the TOF system as claimed in claim 3, characterized in that: In step S4, the cooperative target is searched and matched to obtain an identification result, which is specifically: Through the four target identification principles, the cooperative targets are identified in turn.

5. The method for measuring relative position and posture of space cooperative targets based on the TOF system as claimed in claim 4, characterized in that: In the step S5, the posture solution of the current frame is performed according to the recognition result and the posture solution result of the previous frame, which is specifically: if the recognition result is successful, the posture solution of the current frame is performed; if the recognition result is successful and the posture solution result of the previous frame is completed, the target is tracked according to the posture calculated in the previous frame, the coordinate position of the target is back-projected, the center of mass of the cooperative target is obtained, and the posture calculation of the current frame is performed.

6. The method for measuring relative position and posture of space cooperative targets based on the TOF system as claimed in claim 5, characterized in that: The posture solution of the current frame specifically includes: solving the rotation matrix through iteration according to the collinear equation and the indirect adjustment principle.

Images