Multi-target body adaptive pose measurement algorithm

Abstract

The present application relates to a kind of multi-target body adaptive pose measurement algorithm, belong to on-orbit multi-target body identity intelligent identification and pose measurement field;Any 4 points are taken on target body coordinate system, the coordinates of A1, A2, A3, A4 are expressed as matrix form,,,, ; The deviation of target body coordinate system to parent body coordinate system is expressed as matrix form T;With continuous rotation matrix W, 4 points A1, A2, A3, A4 under target body coordinate system are converted into parent body coordinate system, and the corresponding points are D1, D2, D3, D4 respectively;The coordinate values corresponding to D1, D2, D3, D4 are calculated;Through the calibration correction and the calibration correction of temperature to random measurement error in the setting measurement process, the final accurate measurement value is obtained;The present application realizes multi-target body simultaneous identification and measurement, and is not affected by illumination.

Description

Technical Field

[0001] This invention belongs to the field of on-orbit multi-object identity intelligent recognition and pose measurement, and relates to a multi-object adaptive pose measurement algorithm. Background Technology

[0002] Currently, single-unit space platforms are limited by their carrying capacity and launch capabilities, making it difficult to meet the application requirements of space missions. Establishing common technologies for large-scale, orbit-assembleable spacecraft structural mechanisms, and achieving modularization and functionalization of spacecraft structures, along with the flexible on-orbit assembly and reconfiguration of structural modules, is a crucial direction for future space technology development. New types of on-orbit, assembleable and reconfigurable spacecraft platforms, represented by "swarm" micro-nano satellites with on-orbit multi-body modular components, large-scale scalable space mechanisms, and on-orbit service platforms, play an irreplaceable role in space offense and defense missions and on-orbit service missions requiring rapid response to multiple targets. Due to the characteristics of multi-system, multi-module, and multi-sensor collaborative operation, accurate identification, attitude measurement, and adjustment are key to the construction and application of on-orbit, assembleable and reconfigurable spacecraft platform systems. Currently, conventional technologies for identification and attitude measurement of on-orbit multi-target modular components mainly include radar measurement and visual measurement methods. Radar and visual measurement methods are basically limited to target pose measurement in one-to-one docking and separation scenarios. They have significant limitations in target identification, pose measurement, and localization during on-orbit acquisition, docking, and assembly of multiple targets such as "one parent body + multiple daughter bodies" or "multiple parent bodies + multiple daughter bodies". Conventional laser ranging and image recognition methods face challenges such as large size and weight, line-of-sight obstruction, and difficulty in identification when dealing with multiple daughter bodies at close range. These methods have bottlenecks in applications such as close-range constellation localization, identification, and attitude adjustment in space attack and defense and on-orbit servicing missions. Summary of the Invention

[0003] The technical problem solved by this invention is to overcome the shortcomings of the prior art and propose a multi-target adaptive pose measurement algorithm, which realizes the simultaneous identification and measurement of multiple targets and is not affected by lighting.

[0004] The solution of the present invention is:

[0005] A multi-target adaptive pose measurement algorithm includes:

[0006] Step 1: Establish a parent coordinate system on the parent body. Establish a target coordinate system on the target body. );

[0007] Step 2: Set the coordinates of the origin of the parent coordinate system to A in the target coordinate system. ); arbitrarily select four non-coplanar points A1, A2, A3, and A4 on the target body coordinate system, with coordinates of A1( ), A2 ), A3 ), A4 ); measure the distance l1, l2, l3, l4 of each point A1, A2, A3, A4 from the origin A of the parent coordinate system;

[0008] Step three, according to l1, l2, l3, l4, establish the spatial distance equation, calculate the specific coordinate value of A ); wherein, , , corresponding to the deviation of the parent coordinate system to the target coordinate system , , ;

[0009] Step four, define the target coordinate system relative to the parent coordinate system without deflection only distance deviation, that is, the three axis deflection angles of the two coordinate systems are all 0; set the three-axis deflection angles of the target body itself in the parent coordinate system as ; calculate the rotation matrix of the target body itself converted to the parent coordinate system ) around the x-axis , calculate the rotation matrix of the target body itself converted to the parent coordinate system ) around the y-axis , calculate the rotation matrix of the target body itself converted to the parent coordinate system ) around the z-axis ; and calculate the continuous rotation matrix W according to , , ;

[0010] Step five, express the coordinates of A1, A2, A3, A4 as matrix form , , , ; express the deviation of the target coordinate system to the parent coordinate system as matrix form T; combine the continuous rotation matrix W in step four, convert the four points A1, A2, A3, A4 in the target coordinate system to the parent coordinate system, and the corresponding points are D1, D2, D3, D4; calculate the coordinate values corresponding to D1, D2, D3, D4;

[0011] Step six, establish a distance equation group of four points from the origin of the parent coordinate system according to the coordinate values corresponding to D1, D2, D3, D4;

[0012] Step seven, calculate the three-axis deflection angles of the target body itself in the parent coordinate system ;

[0013] Step eight, randomly select m points on the target body coordinate system; m is an integer multiple of 4; every 4 points form a group, repeat steps two to five to obtain the coordinates v1, v2, v3, …, v m of each point in the parent body coordinate system m ; calculate the average value of v1, v2, v3, …, v m , denoted as ; calculate the deviation of the measured coordinates ; according to the normal distribution 3𝜎 principle, remove the test results outside the range of ±3 times the standard deviation from v1, v2, v3, …, v n , and set k points are removed

[0014] Set the error , calibrate the remaining m-k points according to , and the calibration value is ;

[0015] Step nine, set the temperature corresponding to each time during the measurement as T1, …, T n ; the deformation amount corresponding to each temperature T1, …, T n ; establish a linear regression equation of T i and W i ; the coordinates after temperature calibration are denoted as , ; and the final measurement value is .

[0016] In the above-mentioned multi-target body adaptive pose measurement algorithm, in step one, when establishing the parent body coordinate system , an arbitrary point is selected as the coordinate origin of the parent body coordinate system to ensure that the x-axis, y-axis and z-axis of the parent body coordinate system are perpendicular to each other

[0017] When establishing the child body coordinate system , an arbitrary point is selected as the coordinate origin of the child body coordinate system to ensure that the x-axis, y-axis and z-axis of the child body coordinate system are perpendicular to each other

[0018] In the above-mentioned multi-target body adaptive pose measurement algorithm, in step three, the spatial distance equation is:

[0019] (1)

[0020] Subtract formula (1) from each other and perform matrix transformation to obtain the specific coordinate value of A .

[0021] ​​​In the multi-target body adaptive pose measurement algorithm, in step four, the rotation matrix around the x-axis is is:

[0022] (2)

[0023] The rotation matrix around the y-axis is is:

[0024] (3)

[0025] The rotation matrix around the z-axis is is:

[0026] (4)

[0027] The continuous rotation matrix W is

[0028] W = PQR (5).

[0029] In the multi-target body adaptive pose measurement algorithm, in step five, , , , Specifically,

[0030]

[0031]

[0032]

[0033] (6)

[0034] T is:

[0035] (7).

[0036] In the multi-target body adaptive pose measurement algorithm, the coordinates D1, D2, D3, D4 are specifically:

[0037] (8).

[0038] In the multi-target body adaptive pose measurement algorithm, the four-point distance from the parent coordinate system origin distance equation set is:

[0039] (9)

[0040] In the formula, D i is the distance from the D point to the origin of the parent coordinate system; therefore iThe square root of the sum of squares of the three items in the formula (1) is equal to .

[0041] In the multi-target body adaptive pose measurement algorithm, in step seven, according to the continuous rotation matrix W of step four, the coordinate values corresponding to D1, D2, D3 and D4 of step five, the distance equation group of step six, formulas (2), (3), (4), (5), (8) and (9) are used to calculate the three-axis deflection angle of the target body in the parent coordinate system .

[0042] In the multi-target body adaptive pose measurement algorithm, in step eight, the deviation of the measurement coordinate is .

[0043]

[0044] Error .

[0045]

[0046] Calibration value .

[0047] .

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

[0049]

[0050] In the formula, and are coefficients to be solved;

[0051] is residual error;

[0052] A plurality of groups of measured T i and W i are brought into the above equation, and is eliminated by subtraction to obtain and , and the final linear regression equation is obtained.

[0053] The beneficial effects of the present application compared with the prior art are:

[0054] (1) The present application is based on the high-precision, general on-orbit multi-target body identity intelligent recognition and pose measurement algorithm of radio frequency technology, which solves the problems of high-precision multi-target body identity recognition and pose measurement in the process of on-orbit capture, docking, assembly and other processes of large-scale, expandable and reconfigurable spacecrafts;

[0055] (2) Through the above method, the present invention realizes the target identification, pose measurement and positioning of multiple target objects in the process of on-orbit capture, docking and assembly of multiple target objects such as "one parent body + multiple target objects" and "multiple parent bodies + multiple target objects";

[0056] (3) The present invention achieves high-precision multi-target synchronous measurement in an on-orbit environment and is not affected by light. Attached Figure Description

[0057] Figure 1 This is a flowchart of the multi-target adaptive pose measurement process of the present invention. Detailed Implementation

[0058] The present invention will be further described below with reference to the embodiments.

[0059] This invention provides a multi-target adaptive pose measurement algorithm, which is a high-precision, universal on-orbit multi-target identity intelligent recognition and pose measurement algorithm based on radio frequency technology. It solves the problems of high-precision multi-target identity recognition and pose measurement in the on-orbit capture, docking and assembly processes of large-scale, scalable and reconfigurable spacecraft.

[0060] Multi-object adaptive pose measurement algorithms, such as Figure 1 As shown, the specific steps include the following:

[0061] Step 1: Establish a parent coordinate system on the parent body. Establish a target coordinate system on the target body. Establish the parent coordinate system. When establishing a child coordinate system, select any point as the origin of the parent coordinate system, ensuring that the x-axis, y-axis, and z-axis in the parent coordinate system are perpendicular to each other; When selecting any point as the origin of the sub-body coordinate system, ensure that the sub-body coordinate system... axis, axis, The axes are perpendicular to each other.

[0062] Step 2: Set the coordinates of the origin of the parent coordinate system to A in the target coordinate system. ); arbitrarily select four non-coplanar points A1, A2, A3, and A4 on the target body coordinate system, with coordinates of A1( A2 A3 A4 ); Measure the distances l1, l2, l3, l4 of each point A1, A2, A3, A4 from the origin A of the parent coordinate system.

[0063] Step 3: Establish the spatial distance equation based on l1, l2, l3, and l4, and calculate A ( ) the specific coordinate values of A1, A2, A3, A4; wherein, , , respectively correspond to the deviation of the parent coordinate system to the target coordinate system , , . The spatial distance equation is:

[0064] (1)

[0065] Subtracting formula (1) in turn, matrix transformation is obtained A (1) ) the specific coordinate values of A1, A2, A3, A4.

[0066] Step four, define the target coordinate system relative to the parent coordinate system without deviation only distance deviation, that is, the three axis rotation angles of the two coordinate systems are all 0; Set the three-axis rotation angles of the target body itself in the parent coordinate system respectively ; Calculate the target body itself conversion to the parent coordinate system (1) ) around the x-axis rotation matrix , calculate the target body itself conversion to the parent coordinate system (1) ) around the y-axis rotation matrix , calculate the target body itself conversion to the parent coordinate system (1) ) around the z-axis rotation matrix ; And according to , , Calculate the continuous rotation matrix W.

[0067] The rotation matrix around the x-axis is:

[0068] (2)

[0069] The rotation matrix around the y-axis is:

[0070] (3)

[0071] The rotation matrix around the z-axis is:

[0072] (4)

[0073] The continuous rotation matrix W is:

[0074] W=PQR (5).

[0075] Step five, the coordinates of A1, A2, A3, A4 are expressed as matrix form , , , The deviation from the target coordinate system to the parent coordinate system is represented as a matrix T. Combining the continuous rotation matrix W in step four, the four points A1, A2, A3, and A4 in the target coordinate system are transformed to the parent coordinate system, with corresponding points D1, D2, D3, and D4, respectively. The coordinate values ​​corresponding to D1, D2, D3, and D4 are calculated.

[0076] , , , Specifically:

[0077]

[0078]

[0079]

[0080] (6)

[0081] T is:

[0082] (7).

[0083] Step Six: Based on the coordinate values ​​corresponding to D1, D2, D3, and D4, establish a system of equations representing the distances of the four points from the origin of the parent coordinate system. The coordinates D1, D2, D3, and D4 are as follows:

[0084] (8).

[0085] The system of equations for the distances of the four points from the origin of the parent coordinate system is as follows:

[0086] (9)

[0087] In the formula, D represents i The distance from the point to the origin of the parent coordinate system; therefore ;D i The square roots of the sum of the squares of the three terms in the equation are respectively equal to .

[0088] Step 7: Based on the continuous rotation matrix W from Step 4, the coordinate values ​​corresponding to D1, D2, D3, and D4 from Step 5, and the distance equations from Step 6, calculate the three-axis deflection angles of the target object in the parent coordinate system. By combining formulas (2), (3), (4), (5), (8), and (9), the three-axis deflection angles of the target body in the parent coordinate system can be calculated. .

[0089] Step eight, randomly select m points on the target body coordinate system; m is an integer multiple of 4; every 4 points form a group, repeat steps two to five to obtain the coordinates v1, v2, v3, …, v m of each point in the parent body coordinate system m Calculate the average value of v1, v2, v3, …, v m , denoted as Calculate the deviation of the measured coordinates According to the normal distribution 3𝜎 principle, remove the test results outside the range of ±3 times the standard deviation from v1, v2, v3, …, v n , and set k points are removed.

[0090] Set the error , and calibrate the remaining m-k points according to , and the calibration value is .

[0091] The deviation of the measured coordinates is:

[0092]

[0093] The error is:

[0094]

[0095] The calibration value is:

[0096] .

[0097] After calculation, the normal distribution 3𝜎 principle can be used for screening again, and generally there will be no large deviation error data, that is, the total screening times are 2 times. The average value minus the calculated error measurement value is used for error calibration to obtain more accurate results.

[0098] After the coordinates of the origin of the parent body coordinate system in the target body coordinate system are obtained, the distance l from the parent body coordinate system to the target body coordinate system and the coordinates of the origin of the target body coordinate system in the parent body coordinate system are calculated in turn according to steps one to five.

[0099] Step nine, set the temperature corresponding to each time during the measurement as T1, …, T n ; the deformation amount corresponding to each temperature T1, …, T n ; establish a linear regression equation of T i and W i ; the coordinates after temperature calibration are denoted as , ; and the final measurement value is .

[0100] The linear regression equation is:

[0101]

[0102] In the formula, And Are coefficients to be solved;

[0103] Is a residual error;

[0104] A plurality of groups of measured T i And W i Are brought into the above equation, and After eliminating And Are solved, and the final linear regression equation is obtained.

[0105] Also, the calibrated value of Can be re-eliminated and calculated through step eight, and the measurement result is: The q term is eliminated, and the final measurement result is obtained.

[0106] The present application is based on the high precision, general on-orbit multi-target body identity intelligent recognition and pose measurement algorithm of radio frequency technology, solves the high precision multi-target body identity recognition, pose measurement and other problems in the on-orbit capture, docking, assembly and other processes of large-scale, scalable and reconfigurable spacecraft; through the above method, the target body identity recognition, pose measurement and positioning in the on-orbit capture, docking, assembly and other processes of "one mother body + multiple target bodies", "multiple mother bodies + multiple target bodies" and other multi-target bodies are realized; the present application realizes high-precision multi-target synchronous measurement in on-orbit environment, and is not affected by light.

[0107] Although the present application has been disclosed as above with reference to the preferred embodiments, it is not intended to limit the present application, and any person skilled in the art can make possible changes and modifications to the technical solutions of the present application by using the disclosed methods and technical contents without departing from the spirit and scope of the present application, therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present application, which does not deviate from the technical solutions of the present application, all belong to the protection scope of the technical solutions of the present application.

Claims

1. A multi-target body adaptive pose measurement algorithm, characterized in that: Comprise: Step one, establish the parent coordinate system on the parent ( ) ; establish the target coordinate system on the target ( ) ; Step two, set the parent coordinate system origin in the target body coordinate system coordinates for A ); In the target body coordinate system, take four points A1, A2, A3, A4 arbitrarily, 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 parent coordinate system origin A; Step three, according to l1, l2, l3, l4, establish the spatial distance equation, calculate the specific coordinate value of A ); wherein, 、 、 respectively correspond to the deviation of the parent coordinate system to the target coordinate system 、 、 ; Step four, define the target body coordinate system relative to the parent body coordinate system without deflection only distance deviation, that is, the three axis deflection angle of the two coordinate systems are 0; Set the three-axis deflection angle of the target body itself in the parent body coordinate system respectively ; Calculate the rotation matrix of the target body itself converted to the parent body coordinate system ) around the x-axis , calculate the rotation matrix of the target body itself converted to the parent body coordinate system ) around the y-axis , calculate the rotation matrix of the target body itself converted to the parent body coordinate system ) around the z-axis ; and calculate the continuous rotation matrix W according to , , ​ Step five, express the coordinates of A1, A2, A3, A4 in matrix form , , , ; express the deviation of the target body coordinate system to the parent body coordinate system as a matrix form T; combine the continuous rotation matrix W in step four to convert the four points A1, A2, A3, A4 in the target body coordinate system to the parent body coordinate system, and the corresponding points are D1, D2, D3, D4; calculate the coordinate values corresponding to D1, D2, D3, D4; Step six, according to the coordinate value of D1, D2, D3, D4 corresponding to the four point distance from the parent coordinate system origin equation group; Step seven, calculate the three-axis deflection angle of the target body itself in the parent coordinate system ; Step eight, randomly take m points on the target body coordinate system; m is an integer multiple of 4; every 4 points is a group, repeat steps two to five to obtain the coordinates v1, v2, v3, …, v m of each point in the parent body coordinate system m ; calculate the average value of v1, v2, v3, …, v ; calculate the deviation of the measured coordinates ; according to the normal distribution principle, eliminate the test results outside the range of ±3 times the standard deviation from v1, v2, v3, …, v m , and set k points are eliminated; Set error , according to calibration for the remaining m-k points, the calibration value is ; Step nine, set the measurement process, each time corresponding to the temperature is T1, …, T n ; each temperature T1, …, T n corresponding to the deformation variable is W1, …, W n ; the linear regression equation of T i and W i ; the temperature calibrated coordinates are marked as , ; the final measurement value is .

2. The multi-target body adaptive pose measurement algorithm according to claim 1, characterized in that: In the step one, when establishing the parent coordinate system, ) an arbitrary point is selected as the parent coordinate system coordinate origin, and the x-axis, y-axis and z-axis of the parent coordinate system are perpendicular to each other.

3. The multi-target body adaptive pose measurement algorithm of claim 1, wherein: In the step three, the space distance equation is: （1） By successively subtracting and performing matrix transformations on formula (1), we obtain A( The specific coordinates of ).

4. The multi-target body adaptive pose measurement algorithm of claim 1, wherein: In step four, the rotation matrix about the x-axis is: （2） Rotation matrix around y-axis is: （3） Rotation matrix around z-axis is: （4） The continuous rotation matrix W is: W=PQR (5).

5. The multi-target body adaptive pose measurement algorithm of claim 4, wherein: In step five, , , , Specifically: （6） T is: （7）。 6. The multi-target body adaptive pose measurement algorithm of claim 5, wherein: The coordinates D1, D2, D3, D4 are specifically: （8）。 7. The multi-target body adaptive pose measurement algorithm of claim 6, wherein: The four point distance from the parent coordinate system origin equation group is: （9） wherein represents D i distance from the origin of the maternal coordinate system; thus ; D i the square root of the sum of the squares of the three terms in .

8. The multi-target body adaptive pose measurement algorithm of claim 7, wherein: In the step seven, according to the continuous rotation matrix W of the step four, the coordinate values corresponding to D1, D2, D3, D4 of the step five, the distance equation group of the step six, the formula (2), (3), (4), (5), (8), (9) are solved to obtain the three-axis deflection angle of the target body in the parent coordinate system .

9. The multi-target body adaptive pose measurement algorithm of claim 8, wherein: In step eight, the deviation of the measured coordinates is determined is: Error is: Calibration value is: 。 10. The multi-target body adaptive pose measurement algorithm of claim 8, wherein: In the step nine, the linear regression equation is: wherein and are coefficients to be determined. is a residual; The measured T i and W i are brought into the above equation, and the subtraction eliminates and the final linear regression equation is obtained. and ​

Images