Multi-target adaptive pose measurement algorithm

Abstract

The invention relates to a multi-target adaptive pose measurement algorithm, and belongs to the field of in-orbit multi-target identity intelligent identification and pose measurement. The method comprises the following steps: randomly taking four points on a coordinate system of a target body, and representing coordinates of A1, A2, A3 and A4 as a matrix form # imgabs0 #, # imgabs1 #, # imgabs2 # and # imgabs3 #; representing the deviation from the target body coordinate system to the parent coordinate system as a matrix form T; in combination with the continuous rotation matrix W, four points A1, A2, A3 and A4 in a target body coordinate system are converted into a parent coordinate system, and the corresponding points are D1, D2, D3 and D4 respectively; calculating coordinate values corresponding to D1, D2, D3 and D4; a final accurate measurement value is obtained through calibration and correction of random measurement errors and calibration and correction of temperature in the set measurement process; according to the invention, simultaneous identification and measurement of multiple target bodies are realized without being influenced by illumination.

Description

Technical Field

[0001] The invention belongs to the field of on-orbit multi-target intelligent identity recognition and posture measurement, and relates to a multi-target adaptive posture measurement algorithm. Background Art

[0002] At present, due to the limitations of carrying capacity and transportation, single space platforms are unable to meet the application requirements of space missions. Establishing common technologies for large-scale spacecraft structures that can be assembled on-orbit, realizing modularization and functionalization of spacecraft structures, and flexible assembly and reconstruction of structural modules on-orbit are important development directions for future space technology. New on-orbit assembleable and reconfigurable spacecraft platforms represented by "swarm" micro-nano satellites with on-orbit multi-body combined modules, large space expandable mechanisms, and on-orbit service platforms play an irreplaceable role in space attack and defense and on-orbit service missions that respond rapidly to multiple space targets. Due to the characteristics of multi-system, multi-module, and multi-sensor collaborative work, accurate identity recognition, posture measurement, and adjustment are the key to the construction and application of on-orbit assembleable and reconfigurable spacecraft platform systems. The conventional technical means currently used for identity recognition and posture measurement of on-orbit multi-target combined modules mainly include radar measurement and visual measurement methods. Radar measurement and visual measurement methods are basically limited to the measurement of target body posture in one-to-one docking and separation conditions. They have great limitations in target body identity recognition, posture measurement and positioning during the on-orbit capture, docking and assembly of multiple targets such as "one mother body + multiple daughter bodies" and "multiple mother bodies + multiple daughter bodies". Conventional laser ranging and image recognition methods have difficulties in the case of multiple daughter bodies at close range, such as large size and weight, blocked line of sight, and difficult identity recognition. There are bottlenecks in their application in close-range star cluster positioning, recognition, attitude adjustment and other aspects in space attack and defense, on-orbit service and other tasks. Summary of the invention

[0003] The technical problem solved by the present invention is: to overcome the deficiencies of the prior art and propose a multi-target adaptive posture measurement algorithm, which realizes the simultaneous recognition and measurement of multiple targets and is not affected by illumination.

[0004] The solution of the present invention is: A multi-target adaptive pose measurement algorithm, comprising: Step 1: Establish the mother coordinate system on the mother body ( );Establish the target body coordinate system on the target body ( ); Step 2: Set the coordinate of the origin of the parent coordinate system in the target coordinate system to A ( ) ; Take 4 non-coplanar points A at random in the target body coordinate system 1 , A 2 , A 3 , A 4 The coordinates of each point are A1 ( ), A 2 ( ), A 3 ( ) 、A 4 ( );Measure each point A 1 , A 2 , A 3 , A 4 The distance l from the origin A of the mother coordinate system 1 , l 2 , l 3 , l 4 ; Step 3: According to l 1 , l 2 , l 3 , l 4 Establish the spatial distance equation and calculate A ( ) specific coordinate values; where, , , Corresponding to the deviation from the parent body coordinate system to the target body coordinate system , , ; Step 4: Define the target coordinate system with respect to the parent coordinate system. There is no deflection but only distance deviation, that is, the deflection angle of the three axes of the two coordinate systems. are all 0; set the three-axis deflection angles of the target body itself in the parent body coordinate system to be ; Calculate the target body's own transformation to the parent body coordinate system ( ) Rotation matrix around the x-axis , calculate the target body itself to transform to the parent body coordinate system ( ) Rotation matrix around the y axis , calculate the target body itself to transform to the parent body coordinate system ( ) Rotation matrix around the z axis ; and according to , , Calculate the continuous rotation matrix W; Step 5: A 1 , A 2 , A 3 , A 4 The coordinates are expressed in matrix form , , , ; Express the deviation from the target coordinate system to the mother coordinate system as a matrix form T; Combined with the continuous rotation matrix W in step 4, the four points A in the target coordinate system 1 , A2 , A 3 , A 4 Transformed to the mother coordinate system, the corresponding points are D 1 , D 2 , D 3 , D 4 ; Calculate D 1 , D 2 , D 3 , D 4 The corresponding coordinate values; Step 6: According to D 1 , D 2 , D 3 , D 4 The corresponding coordinate values ​​establish the distance equations of the four points from the origin of the mother coordinate system; Step 7: Calculate the three-axis deflection angle of the target body in the parent body coordinate system ; Step 8. Randomly select m points in the target body coordinate system; m is an integer multiple of 4; every 4 points form a group, repeat steps 2 to 5 to obtain the coordinates v of each point in the parent body coordinate system 1 、v 2 、v 3 , ..., v m ; Calculate v 1 、v 2 、v 3 , ..., v m The average value of ; Calculate the deviation of the measured coordinates ; According to the normal distribution 3𝜎 principle, from v 1 、v 2 、v 3 , ..., v m The test results that are outside the range of ±3 times the standard deviation of the mean value are eliminated, assuming that k points are eliminated; Setting error ,according to Calibrate the remaining mk points, and the calibration value is ; Step 9: Set the temperature corresponding to each moment in the measurement process as T 1 ,……,T n ; Each temperature T 1 ,……,T n The corresponding deformation variable is W 1 ,……,W n ; Establish T i and W i The linear regression equation for temperature correction is , The final measurement value is .

[0005] In the above-mentioned multi-target adaptive pose measurement algorithm, in step 1, a parent coordinate system is established ( ), select any point as the origin of the parent coordinate system to ensure that the x-axis, y-axis, and z-axis in the parent coordinate system are perpendicular to each other; Establish the sub-body coordinate system ( ), select any point as the origin of the sub-body coordinate system to ensure that axis, axis, The axes are perpendicular to each other.

[0006] In the above-mentioned multi-target adaptive pose measurement algorithm, in step 3, the spatial distance equation is: (1) Subtract and transform the matrix of formula (1) to obtain A ( )’s specific coordinate values.

[0007] In the above-mentioned multi-target adaptive posture measurement algorithm, in step 4, the matrix is ​​rotated around the x-axis for: (2) Rotation matrix around the y axis for: (3) Rotation matrix around the z axis for: (4) The continuous rotation matrix W is: W=PQR(5).

[0008] In the above-mentioned multi-target adaptive pose measurement algorithm, in step 5, , , , Specifically:

[0009]

[0010]

[0011] (6) T is: (7).

[0012] In the above-mentioned multi-target adaptive pose measurement algorithm, the coordinate D 1 , D 2 , D 3 , D 4 Specifically: (8).

[0013] In the above-mentioned multi-target adaptive pose measurement algorithm, the distance equations of the four points from the origin of the parent coordinate system are: (9) In the formula, Indicates D i The distance from the point to the origin of the parent coordinate system; therefore ;D i The sum of the square roots of the three terms in is equal to .

[0014] In the above-mentioned multi-target adaptive pose measurement algorithm, in step 7, according to the continuous rotation matrix W of step 4 and D of step 5, 1 , D 2 , D 3 , D 4 The corresponding coordinate values ​​and the distance equation group in step 6 are combined with formulas (2), (3), (4), (5), (8), and (9) to calculate the three-axis deflection angle of the target body in the parent coordinate system. .

[0015] In the above-mentioned multi-target adaptive pose measurement algorithm, in step eight, the deviation of the measured coordinates is for:

[0016] error for:

[0017] Calibration value for: .

[0018] In the above-mentioned multi-target adaptive pose measurement algorithm, in step nine, the linear regression equation is:

[0019] In the formula, and All are coefficients to be determined; is the residual; Multiple groups of measured T i and Wi Substitute into the above equation and eliminate Afterwards and , and obtain the final linear regression equation.

[0020] The beneficial effects of the present invention compared with the prior art are: (1) The high-precision, universal on-orbit multi-target identity intelligent recognition and posture measurement algorithm based on radio frequency technology in this invention solves the difficult problems of high-precision multi-target identity recognition and posture measurement in the on-orbit capture, docking, and assembly of large-scale, scalable, and reconfigurable spacecraft; (2) The present invention realizes the target identification, posture measurement and positioning in the process of on-orbit capture, docking and assembly of multiple targets such as "one parent body + multiple targets" and "multiple parent bodies + multiple targets" through the above method; (3) The present invention realizes high-precision multi-target synchronous measurement in an on-orbit environment and is not affected by light. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 This is a flow chart of the adaptive posture measurement of multiple targets of the present invention. DETAILED DESCRIPTION

[0022] The present invention will be further described below in conjunction with the embodiments.

[0023] The present invention provides a multi-target adaptive posture measurement algorithm, which is a high-precision, universal on-orbit multi-target identity intelligent recognition and posture measurement algorithm based on radio frequency technology, and solves the difficult problems of high-precision multi-target identity recognition and posture measurement during the on-orbit capture, docking, and assembly of large-scale, scalable, and reconfigurable spacecraft.

[0024] Multi-target adaptive pose measurement algorithm, such as Figure 1 As shown, the specific steps include: Step 1: Establish the mother coordinate system on the mother body ( );Establish the target body coordinate system on the target body ( ). Establish the mother coordinate system ( ), select any point as the origin of the parent coordinate system to ensure that the x-axis, y-axis, and z-axis in the parent coordinate system are perpendicular to each other; establish the child coordinate system ( ), select any point as the origin of the sub-body coordinate system to ensure that axis, axis, The axes are perpendicular to each other.

[0025] Step 2: Set the coordinate of the origin of the parent coordinate system in the target coordinate system to A ( ) ; Take 4 non-coplanar points A at random in the target body coordinate system 1 , A 2 , A 3 , A 4 The coordinates of each point are A 1 ( ), A 2 ( ), A 3 ( ) 、A 4 ( );Measure each point A 1 , A 2 , A 3 , A 4 The distance l from the origin A of the mother coordinate system 1 , l 2 , l 3 , l 4 .

[0026] Step 3: According to l 1 , l 2 , l 3 , l 4 Establish the spatial distance equation and calculate A ( ) specific coordinate values; where, , , Corresponding to the deviation from the parent body coordinate system to the target body coordinate system , , . The spatial distance equation is: (1) Subtract and transform the matrix of formula (1) to obtain A ( )’s specific coordinate values.

[0027] Step 4: Define the target coordinate system with respect to the parent coordinate system. There is no deflection but only distance deviation, that is, the deflection angle of the three axes of the two coordinate systems. are all 0; set the three-axis deflection angles of the target body itself in the parent body coordinate system to be ; Calculate the target body's own transformation to the parent body coordinate system ( ) Rotation matrix around the x-axis , calculate the target body itself to transform to the parent body coordinate system ( ) Rotation matrix around the y axis , calculate the target body itself to transform to the parent body coordinate system ( ) Rotation matrix around the z axis ; and according to , , Compute the continuous rotation matrix W.

[0028] Rotation matrix around the x-axis for: (2) Rotation matrix around the y axis for: (3) Rotation matrix around the z axis for: (4) The continuous rotation matrix W is: W=PQR(5).

[0029] Step 5: A 1 , A 2 , A 3 , A 4 The coordinates are expressed in matrix form , , , ; Express the deviation from the target coordinate system to the mother coordinate system as a matrix form T; Combined with the continuous rotation matrix W in step 4, the four points A in the target coordinate system 1 , A 2 , A 3 , A 4 Transformed to the mother coordinate system, the corresponding points are D 1 , D 2 , D 3 , D 4 ; Calculate D 1 , D 2 , D 3 , D 4 The corresponding coordinate values.

[0030] , , , Specifically:

[0031]

[0032]

[0033] (6) T is: (7).

[0034] Step 6: According to D 1 , D 2 , D3 , D 4 The corresponding coordinate values ​​establish the distance equations of the four points from the origin of the mother coordinate system. 1 , D 2 , D 3 , D 4 Specifically: (8).

[0035] The distance equations of the four points from the origin of the mother coordinate system are: (9) In the formula, Indicates D i The distance from the point to the origin of the parent coordinate system; therefore ;D i The sum of the square roots of the three terms in is equal to .

[0036] Step 7: Based on the continuous rotation matrix W of step 4 and D of step 5 1 , D 2 , D 3 , D 4 The corresponding coordinate values ​​and the distance equation group in step 6 are used to calculate the three-axis deflection angle of the target body in the parent coordinate system. By combining formulas (2), (3), (4), (5), (8), and (9), we can calculate the three-axis deflection angle of the target body in the parent coordinate system. .

[0037] Step 8. Randomly select m points in the target body coordinate system; m is an integer multiple of 4; every 4 points form a group, repeat steps 2 to 5 to obtain the coordinates v of each point in the parent body coordinate system 1 、v 2 、v 3 , ..., v m ; Calculate v 1 、v 2 、v 3 , ..., v m The average value of ; Calculate the deviation of the measured coordinates ; According to the normal distribution 3𝜎 principle, from v 1 、v 2 、v 3 , ..., v m The test results that are outside the range of ±3 times the standard deviation of the mean value are eliminated, and k points are eliminated.

[0038] Setting error ,according to Calibrate the remaining mk points, and the calibration value is .

[0039] Deviation of measuring coordinates for:

[0040] error for:

[0041] Calibration value for: .

[0042] After the calculation, the normal distribution 3𝜎 principle can be used for screening again. Generally, there will be no large offset error data, that is, the total number of screenings is 2. The average value minus the calculated error measurement value is used for error calibration to obtain a more accurate result.

[0043] After finding the coordinates of the origin of the mother coordinate system in the target coordinate system, loop steps 1 to 5 to calculate the distance l from the mother coordinate system to the target coordinate system, and the coordinates of the origin of the target coordinate system in the mother coordinate system.

[0044] Step 9: Set the temperature corresponding to each moment in the measurement process as T 1 ,……,T n ; Each temperature T 1 ,……,T n The corresponding deformation variable is W 1 ,……,W n ; Establish T i and W i The linear regression equation for temperature correction is , The final measurement value is .

[0045] The linear regression equation is:

[0046] In the formula, and All are coefficients to be determined; is the residual; Multiple groups of measured T i and W i Substitute into the above equation and eliminate Afterwards and , and obtain the final linear regression equation.

[0047] You can also continue to The calibrated value is eliminated and recalculated in step 8, and the measurement result is: Eliminate the q term and you will get the final measurement result.

[0048] The present invention provides a high-precision, universal on-orbit multi-target identity intelligent recognition and posture measurement algorithm based on radio frequency technology, which solves the difficult problems of high-precision multi-target identity recognition and posture measurement during on-orbit capture, docking, assembly, etc. of large-scale, scalable, and reconfigurable spacecraft; through the above method, target identity recognition, posture measurement and positioning of multi-targets such as "one mother body + multiple targets" and "multiple mother bodies + multiple targets" during on-orbit capture, docking, assembly, etc. are realized; the present invention realizes high-precision multi-target synchronous measurement in an on-orbit environment, and is not affected by light.

[0049] Although the present invention has been disclosed as above in the form of a preferred embodiment, it is not intended to limit the present invention. Any person skilled in the art may make possible changes and modifications to the technical solution of the present invention by using the methods and technical contents disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention shall fall within the protection scope of the technical solution of the present invention.

Claims

1. A multi-target adaptive pose measurement algorithm, characterized by: include: Step 1: Establish the mother coordinate system on the mother body ( );Establish the target body coordinate system on the target body ( ); Step 2: Set the coordinate of the origin of the parent coordinate system in the target coordinate system to A ( ); Take 4 non-coplanar points A1, A2, A3, A4 at random in the target body coordinate system, and the coordinates of each point are A1 ( )、A2 ( )、A3( ) 、A4 ( ); measure the distance l1, l2, l3, l4 of each point A1, A2, A3, A4 from the origin A of the mother coordinate system; Step 3: Establish the spatial distance equation based on l1, l2, l3, and l4, and calculate A ( ) specific coordinate values; where, , , Corresponding to the deviation from the parent body coordinate system to the target body coordinate system , , ; Step 4: Define the target coordinate system with respect to the parent coordinate system. There is no deflection but only distance deviation, that is, the deflection angle of the three axes of the two coordinate systems. are all 0; set the three-axis deflection angles of the target body in the parent body coordinate system to be ; Calculate the target body's own transformation to the parent body coordinate system ( ) Rotation matrix around the x-axis , calculate the target body itself to transform to the parent body coordinate system ( ) Rotation matrix around the y axis , calculate the target body itself to transform to the parent body coordinate system ( ) Rotation matrix around the z axis ; and according to , , Calculate the continuous rotation matrix W; Step 5: Express the coordinates of A1, A2, A3, and A4 in matrix form , , , ; Express the deviation from the target coordinate system to the mother coordinate system in matrix form T; Combined with the continuous rotation matrix W in step 4, transform the four points A1, A2, A3, and A4 in the target coordinate system to the mother coordinate system, and the corresponding points are D1, D2, D3, and D4 respectively; Calculate the coordinate values ​​corresponding to D1, D2, D3, and D4; Step 6: According to the coordinate values ​​corresponding to D1, D2, D3, and D4, establish a distance equation group of the four points from the origin of the mother coordinate system; Step 7: Calculate the three-axis deflection angle of the target body in the parent body coordinate system ; Step 8. Randomly select m points in the target body coordinate system; m is an integer multiple of 4; every 4 points form a group, repeat steps 2 to 5 to obtain the coordinates v1, v2, v3, ..., v of each point in the parent body coordinate system. m ; Calculate v1, v2, v3, ..., v m The average value of ; Calculate the deviation of the measured coordinates ; According to the normal distribution 3𝜎 principle, from v1, v2, v3, ..., v m The test results that are outside the range of ±3 times the standard deviation of the mean value are eliminated, assuming that k points are eliminated; Setting error ,according to Calibrate the remaining mk points, and the calibration value is ; Step 9: Set the temperature at each moment in the measurement process as T1, ..., T n ; Each temperature T1, ..., T n The corresponding deformation variables are W1, ..., W n ; Establish T i and W i The linear regression equation for temperature correction is , The final measurement value is .

2. The method according to claim 1, wherein: In the step 1, the mother coordinate system is established ( ), select any point as the origin of the parent coordinate system to ensure that the x-axis, y-axis, and z-axis in the parent coordinate system are perpendicular to each other; Establish the sub-body coordinate system ( ), select any point as the origin of the sub-body coordinate system to ensure that axis, axis, The axes are perpendicular to each other.

3. The multi-target adaptive pose measurement algorithm according to claim 1, characterized in that: In step 3, the spatial distance equation is: （1） Subtract and transform the matrix of formula (1) to obtain A ( )’s specific coordinate values.

4. The multi-target adaptive pose measurement algorithm according to claim 1, characterized in that: In step 4, the matrix is ​​rotated around the x-axis for: （2） Rotation matrix around the y axis for: （3） Rotation matrix around the z axis for: （4） The continuous rotation matrix W is: W=PQR(5).

5. The multi-target adaptive pose measurement algorithm according to claim 4, characterized in that: In the step five, , , , Specifically: （6） T is: （7）。 6. The multi-target adaptive pose measurement algorithm according to claim 5, characterized in that: The coordinates D1, D2, D3, and D4 are specifically: （8）。 7. The multi-target adaptive pose measurement algorithm according to claim 6, characterized in that: The distance equations of the four points from the origin of the mother coordinate system are: （9） In the formula, Indicates D i The distance from the point to the origin of the parent coordinate system; therefore ;D i The sum of the square roots of the three terms in is equal to .

8. The multi-target adaptive pose measurement algorithm according to claim 7, characterized in that: In step 7, according to the continuous rotation matrix W in step 4, the coordinate values ​​corresponding to D1, D2, D3, and D4 in step 5, and the distance equation group in step 6, formulas (2), (3), (4), (5), (8), and (9) are combined to calculate the three-axis deflection angle of the target body itself in the parent coordinate system. .

9. The multi-target adaptive pose measurement algorithm according to claim 8, characterized in that: In step eight, the deviation of the measured coordinates for: error for: Calibration value for: 。 10. The multi-target adaptive pose measurement algorithm according to claim 8, characterized in that: In step nine, the linear regression equation is: In the formula, and All are coefficients to be determined; is the residual; Multiple groups of measured T i and W i Substitute into the above equation and eliminate Afterwards and , and obtain the final linear regression equation.

Images