A method for measuring the initial position and attitude of a detector under test

Abstract

The present application relates to a kind of detector test initial position attitude measurement method, comprising: in the detector setting 4 different faces mark point, establish detector coordinate system;Before test, establish total station in the coordinate of test field coordinate system under;With the detector coordinate system Z axis and test field coordinate system Z axis parallel, the detector is hung in the air of test field;Total station is erected to the position determined in preparation work;According to the position relationship of total station and detector in test field coordinate system, the coordinates of four cooperation marks on the detector in test field coordinate system are obtained;Conversion matrix R and displacement matrix T are established, and the rotation relationship of detector coordinate system and test field coordinate system is obtained, and the attitude angle of detector is solved.The present application solves the problem that detector cannot be measured position attitude parameter in high altitude, and is used for the measurement of initial position and attitude of detector in spacecraft hovering obstacle avoidance and slow descent test.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of extraterrestrial body landing test, and particularly relates to a method for measuring initial position and attitude of a probe in a test. BACKGROUND

[0002] With the continuous development of human space technology, human space exploration activities have been continuously extended from near-Earth space to deeper and farther space. The United States has realized manned lunar landing and has successfully realized Mars landing exploration many times. The process from approaching an extraterrestrial body to landing on the surface of the extraterrestrial body in deep space exploration is called entry, descent and landing (EDL). The EDL plays a crucial role in the success of the entire exploration mission. Taking Mars landing exploration as an example, only 43% of the Mars landing missions have been successful, and most of the missions have failed in the EDL. Therefore, a large number of ground tests are carried out in the process of spacecraft development to evaluate the working performance of the spacecraft in the EDL, and a test field specially built for spacecraft ground landing test is established, in which the landing test field of the Lengly Research Center in the United States is relatively famous.

[0003] Hovering, obstacle avoidance and slow descent test in extraterrestrial body landing test is a comprehensive test for checking whether the spacecraft can find a suitable landing point and perform landing action. In order to verify that the test probe can control the position and attitude according to the design program, the initial position and attitude of the test probe need to be calibrated and compared with the initial position and attitude data of the control system and the external measurement system. The consistency of the data of each system indicates that each system works well, which serves as a basis for subsequent tests. However, the initial position of the test probe is at an altitude of 70 m, and it is difficult for the measurement personnel to approach, so the initial position and attitude parameters of the test probe cannot be obtained by ground measurement means. SUMMARY

[0004] The technical problem solved by the application is to overcome the shortcomings of the prior art and provide a method for measuring the initial position and attitude of a test probe. The method is characterized in that a method for measuring the initial position and attitude of a test probe by space position coordinates and cooperative markers is designed, and the problem of being unable to measure the position and attitude parameters of the test probe at a close distance in the air is solved.

[0005] The technical solution of the application is a method for measuring the initial position and attitude of a test probe, which comprises the following steps:

[0006] The following two preparation works are carried out before the test:

[0007] Firstly, four cooperative markers not coplanar are arranged on the probe to establish a probe coordinate system.

[0008] Second step, through the test field coordinate system and the fixed target position in the test field coordinate system, establishing the coordinates of the total station in the test field coordinate system before the test;

[0009] The operation steps when conducting the test include:

[0010] Third step, suspending the detector above the test field, the Z axis of the detector coordinate system is parallel to the Z axis of the test field coordinate system;

[0011] Fourth step, erecting the total station to the coordinate position of the total station in the test field coordinate system established in the second step;

[0012] Fifth step, measuring the cooperative marks on the detector by the total station, and obtaining the coordinates of the four cooperative marks on the detector in the test field coordinate system according to the positional relationship between the total station and the detector in the test field coordinate system in the third step and the fourth step;

[0013] Sixth step, establishing the conversion matrix R and the displacement matrix T, obtaining the coordinates of the detector coordinate (0, 0, 0) in the test field coordinate system, and obtaining the rotation relationship between the detector coordinate system and the test field coordinate system, and solving the attitude angle of the detector by combining the conversion matrix R and the rotation relationship.

[0014] Further, the specific steps of the first step of preparation are:

[0015] Step 1.1, arranging four cooperative marks on the detector, the arrangement principle is: selecting the positions which have the least influence on the change of the appearance and shape of the detector, the four cooperative marks are not in the same plane and can be seen at the same time; obtaining the coordinates of the four cooperative marks on the detector through the known three-dimensional model of the detector, defined as (x 1t ,y 1t ,z 1t ) to (x 4t ,y 4t ,z 4t ), and obtaining the matrix mat(from):

[0016]

[0017] Step 1.2, the detector coordinate system is defined as: taking the geometric center position of the detector as the origin O', the Z' axis is upward, X', Y' and Z' form a right-handed coordinate system.

[0018] Further, the specific steps of the second step of preparation are:

[0019] Step 2.1, define the test field coordinate system, determine the coordinates of the fixed targets in the test field coordinate system; the specific method is: define the center position of the test field as the origin of the test field coordinate system, define the direction from the origin position to the positive east direction as the X axis of the coordinate system, define the direction from the origin position to the positive north position as the Y axis of the coordinate system, and define the direction from the origin position to the sky as the Z axis of the coordinate system, and select M fixed target positions in the test field, define the selected fixed target positions as (x1, y1, z1) to (xM, yM, zM), M > 5; M M M

[0020] Step 2.2, measure 5 of the M fixed target positions before the test using a total station, and obtain the coordinate information; the specific method is:

[0021] The total station is erected away from the origin of the test field coordinate system, and can measure at least 5 fixed target points in the test field and 4 cooperative markers on the detector; the total station measures the fixed target positions to obtain the distance l i from the total station to the fixed target position and the included angle a i between the total station and the test field horizontal plane, the difference between the height of the fixed target position to the test field ground and the height of the total station to the test field ground is recorded as h i , i = 1 ~ 4, then the coordinates (x, y, z) of the measurement center of the total station in the test field coordinate system are solved by the following equation group:

[0022]

[0023] Further, the coordinates of the four cooperative markers obtained in the fifth step in the test field coordinate system are defined as (x 1tc , y 1tc , z 1tc ) to (x 4tc , y 4tc , z 4tc ), and a detector target matrix mat(to) is obtained:

[0024]

[0025] Further, the coordinates of the detector coordinate (0, 0, 0) obtained in the sixth step in the test field coordinate system are in the form of:

[0026]

[0027] wherein, is the coordinate of the detector origin coordinate (0, 0, 0) in the test field coordinate system to be solved, is , R represents the rotation relationship of the detector coordinate system around the test field coordinate system, and the form is​​​ T represents the coordinates of the origin of the detector coordinate system in the test field coordinate system, in the form of:

[0028] Furthermore, the process of solving for the transformation matrix R in step six includes:

[0029] S1. Calculate the center point of the four points in the matrix mat(to) to obtain the matrix mat(meanto);

[0030]

[0031] in,

[0032]

[0033]

[0034] S2. Calculate the center point of the four points in the matrix mat(from) to obtain mat(meanfrom);

[0035]

[0036] in,

[0037]

[0038]

[0039] S3. Calculate the coordinate matrices mat(to)' and mat(from)' after eliminating the displacement effect;

[0040] mat(to)'=mat(to)-mat(meanto)

[0041] mat(from)'=mat(from)-mat(meanfrom)

[0042] S4. Calculate the smallest unit vector matrices mat(ton) and mat(fromn) for mat(to)' and mat(from)' respectively;

[0043] The calculation method for mat(ton) is: mat(ton) = mat(to)' / sto1,

[0044] in,

[0045] The calculation method for mat(fromn) is: mat(from) = mat(from)' / sto2.

[0046] in,

[0047] S5. Calculate the transformation matrix R;

[0048] After obtaining mat(ton) and mat(fromn), calculate the two orthogonal bases U and V of the mat(ton)*mat(fromn) matrix; after obtaining U and V, R is obtained through R = U*S*V. T The calculation yields V. T Let V be the transpose of V, and S be the identity matrix.

[0049] Furthermore, the method for solving the displacement matrix T in step six is ​​as follows:

[0050] The displacement matrix T is obtained by subtracting (R*mat(meanfrom)) from the mean to the mean. T ) T The calculated value represents the coordinates of the detector origin in the test field coordinate system.

[0051] Furthermore, the rotation relationship between the detector coordinate system and the test field coordinate system is as follows: the elements of the transformation matrix R are matrices composed of trigonometric functions of the rotation angles Ax, Ay, and Az of the detector's X', Y', and Z' coordinate axes about the test field coordinate system X, Y, and Z, specifically:

[0052]

[0053] By combining the transformation matrix R obtained from U and V with the above equation, the values ​​of Ax, Ay, and Az can be calculated, thereby obtaining the rotation angle between the detector coordinate system and the test field coordinate system, i.e., the attitude angle of the detector.

[0054] Furthermore, the experiment was repeated by changing the position of at least one of the M fixed targets, and the accuracy of the detector's initial attitude measurement was verified by repeatedly measuring multiple sets of fixed targets.

[0055] Furthermore, in the second step, a cross-shaped black and white target is set at the fixed target location.

[0056] This invention solves the problem of measuring the initial position and attitude of the probe in spacecraft hovering obstacle avoidance and slow descent tests.

[0057] The advantages of this invention compared to the prior art are:

[0058] This invention employs a rotation matrix construction method to achieve the transformation between detector coordinates and test field coordinates. Applicable to large-scale spacecraft testing, it eliminates the need for total station coordinate system transformation, directly measuring the initial position and attitude of the test verifier by measuring both test field and detector coordinates. This simplifies the measurement process and is beneficial for verifying test results. Attached Figure Description

[0059] Figure 1 This is a flowchart of the initial attitude measurement method according to an embodiment of the present invention;

[0060] Figure 2 This is a diagram showing the relationship between the test field coordinate system and the verifier coordinate system in an embodiment of the present invention.

[0061] Figure 3 This is a schematic diagram of a total station measuring a fixed target point according to an embodiment of the present invention;

[0062] Figure 4 This is a schematic diagram of a black and white target graphic in an embodiment of the present invention. Detailed Implementation

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

[0064] like Figure 1 The diagram shows a flowchart of the detector's initial position and attitude measurement method according to the present invention. Two preparatory steps are performed before the experiment:

[0065] The first step is to place four non-coplanar cooperative markers on the detector and establish the detector coordinate system;

[0066] The second step is to establish the coordinates of the total station in the test field coordinate system before the test, using the test field coordinate system and the fixed target position in the test field coordinate system.

[0067] The specific steps for the first stage of preparation are as follows:

[0068] 1a) Arrange four cooperative markers on the detector, with the following principles: select positions that minimize the impact on the detector's appearance and shape; ensure the four markers are not in the same plane and can be seen simultaneously. Obtain the coordinates of the four cooperative markers on the detector using the known 3D model of the detector, defined as (x... 1t ,y 1t ,z 1t ) to (x 4t ,y 4t ,z 4t And obtain the matrix mat(from).

[0069]

[0070] 1b) The detector coordinate system is defined as follows: with the geometric center of the detector as the origin O', the Z' axis upward, and the X, Y, and Z' axes forming a right-handed coordinate system.

[0071] The specific steps for the second step of preparation are as follows:

[0072] 2a) Define the test field coordinate system and determine the coordinates of the fixed targets within it. Specifically, define the center of the test field ground as the origin of the coordinate system; define the direction pointing east from the origin as the X-axis; the direction pointing north from the origin as the Y-axis; and the direction pointing sky from the origin as the Z-axis. Select 28 fixed target locations within the test field and define these locations as (x1, y1, z1) to (x... 28 ,y 28 ,z 28 The relationship between the test field coordinate system and the detector coordinate system is as follows: Figure 2 As shown.

[0073] 2b) Before the experiment, use a total station to measure the coordinate information of 5 out of 28 fixed target locations; specifically:

[0074] The total station is set up far from the origin of the test field coordinate system and can measure at least 5 fixed target points in the test field as well as 4 cooperative markers on the detector;

[0075] like Figure 3 As shown, the distance l from the total station to the fixed target position is obtained by measuring the position of the fixed target using a total station. i and the angle α between it and the horizontal plane of the test field i The difference in height between the fixed target location and the test field ground and between the total station and the test field ground is denoted as h. i If i = 1 to 4, then the coordinates (x, y, z) of the measurement center of the total station in the test field coordinate system can be solved by equation (1).

[0076]

[0077] In this embodiment, the fixed target position is represented by a black and white target, the target graphic of which is as follows: Figure 4 As shown, the "cross" at the center of the target indicates the position of the fixed target on the test field. In the next test, by changing at least one of the five measured fixed target positions, the accuracy of the initial position and attitude calculation can be verified.

[0078] The operational steps for conducting the experiment include:

[0079] The third step is to suspend the detector above the test field, with the detector's coordinate system Z-axis parallel to the test field's coordinate system Z-axis. According to the definition of the detector's coordinate system, if the origin of the detector's coordinate system coincides with the origin of the test field's coordinate system, and the Z-axis and X-axis coincide, then the Y-axis must also coincide. Therefore, measuring the detector's positional relationship involves calculating the rotation and offset relationship between the detector's coordinate system and the test field's coordinate system.

[0080] Step 4: Set up the total station in the coordinate system of the test field established in preparation 2b), and confirm that you can see the 4 cooperative markers and 5 fixed target positions on the detector.

[0081] Step 5: Using a total station, measure the cooperative markers on the detector. Based on the positional relationship between the total station and the detector in the test field coordinate system obtained in steps 3 and 4, obtain the coordinates of the four cooperative markers on the detector in the test field coordinate system, defined as (x... 1tc ,y 1tc ,z 1tc ) to (x 4tc ,y 4tc ,z 4tc ), thereby obtaining a set of detector target matrices mat(to).

[0082]

[0083] Step 6: Establish the transformation matrix R and the displacement matrix T, establish the transformation relationship between mat(from) and mat(to), and obtain the coordinates of the detector coordinates (0,0,0) in the test field coordinate system as shown in formula (2); and obtain the rotation relationship between the detector coordinate system and the test field coordinate system.

[0084]

[0085] in, Let (0,0,0) be the coordinates of the detector origin (0,0,0) in the test field coordinate system, which needs to be solved. for R represents the origin of the detector coordinate system, and R represents the rotation relationship of the detector coordinate system about the test field coordinate system, in the form of: T represents the coordinates of the origin of the detector coordinate system in the test field coordinate system, in the form of:

[0086] In the method of this invention, the process of solving for R and T includes:

[0087] 1. Calculate the center point of the four points in matrix mat(to) to obtain matrix mat(meanto);

[0088]

[0089] in,

[0090]

[0091]

[0092] 2. Calculate the center point of the four points in the matrix mat(from) to obtain mat(meanfrom);

[0093]

[0094] in,

[0095]

[0096]

[0097] 3. Calculate the coordinate matrices mat(to)' and mat(from)' after eliminating the influence of displacement;

[0098] mat(to)'=mat(to)-mat(meanto)

[0099] mat(from)'=mat(from)-mat(meanfrom)

[0100] 4. Calculate the smallest unit vector matrices mat(ton) and mat(fromn) of mat(to)' and mat(fromn);

[0101] The calculation method for mat(ton) is: mat(ton) = mat(to)' / sto1,

[0102] in,

[0103] The calculation method for mat(fromn) is: mat(from) = mat(from)' / sto2.

[0104] in,

[0105] 5. Calculate the transformation matrix R and the displacement matrix T;

[0106] After obtaining mat(ton) and mat(fromn), calculate the two orthogonal bases U and V of the mat(ton)*mat(fromn) matrix; after obtaining U and V, R is obtained through R = U*S*V T The calculation yields V. T Let V be the transpose of V, and S be the identity matrix.

[0107] The displacement matrix T is obtained by subtracting (R*mat(meanfrom)) from the mean to the mean. T ) T Calculated.

[0108] Among them, X S Y S Z S The coordinates of the detector origin in the test field coordinate system are given.

[0109] The elements in R are matrices composed of trigonometric functions of the detector's X', Y', and Z' coordinate axes rotated around the test field coordinate system X, Y, and Z by the angles Ax, Ay, and Az, respectively.

[0110]

[0111] By combining the R matrix obtained from U and V with the above equation, the values ​​of Ax, Ay, and Az can be calculated, thereby obtaining the rotation angle between the detector coordinate system and the test field coordinate system, i.e., the attitude angle of the detector.

[0112] Example 1

[0113] Using the cooperative markers with known coordinates on the detector, the coordinate matrix mat(from) in the detector coordinate system is obtained as follows, following the first step of the preparatory work:

[0114]

[0115] Before the experiment begins, set up the total station according to the second step of the preparation work, and calculate the total station coordinates based on the known coordinates of the fixed target position.

[0116] After the detector moves to the initial position of the experiment, the cooperation marker on the detector is measured using a total station, and the coordinate matrix mat(to) of the cooperation marker in the test field coordinate system is calculated as follows:

[0117]

[0118] Obtain the rotation matrices R and T through software programming:

[0119]

[0120] The detector coordinate system was calculated using the R matrix, and rotated 116°, 26°, and 180° around the X, Y, and Z axes of the test field coordinate system, respectively.

[0121] Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make possible changes and modifications to the technical solutions of the present invention by utilizing the methods and techniques disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solutions of the present invention shall fall within the protection scope of the technical solutions of the present invention.

Claims

1. A method for measuring the initial position and attitude of a detector during an experiment, characterized in that... Including the following steps: The following two preparatory steps should be taken before the experiment: The first step is to place four non-coplanar cooperative markers on the detector and establish the detector coordinate system; The second step is to establish the coordinates of the total station in the test field coordinate system before the test, using the test field coordinate system and the fixed target position in the test field coordinate system. The operational steps for conducting the experiment include: The third step is to suspend the detector above the test site, with the Z-axis of the detector coordinate system parallel to the Z-axis of the test site coordinate system. Step 4: Set up the total station in the coordinate system of the test field established in Step 2; Step 5: Use a total station to measure the cooperation marks on the detector. Based on the positional relationship between the total station and the detector in the test field coordinate system obtained in Steps 3 and 4, obtain the coordinates of the four cooperation marks on the detector in the test field coordinate system. Step 6: Establish the transformation matrix R and the displacement matrix T to obtain the coordinates of the detector (0,0,0) in the test field coordinate system, and obtain the rotation relationship between the detector coordinate system and the test field coordinate system. By combining the transformation matrix R and the rotation relationship, the attitude angle of the detector is calculated.

2. The method for measuring the initial position and attitude of a detector experiment according to claim 1, characterized in that, The specific steps of the first preparatory work are as follows: Step 1.1: Arrange four cooperative markers on the detector. The arrangement principle is: select positions that have the least impact on the appearance and shape of the detector; the four cooperative markers should not be in the same plane and should be visible simultaneously; obtain the coordinates of the four cooperative markers on the detector using the known 3D model of the detector, defined as (x... 1t ,y 1t ,z 1t ) to (x 4t ,y 4t ,z 4t ), and obtain the matrix mat(from): Step 1.2: The detector coordinate system is defined as follows: with the geometric center of the detector as the origin O', the Z' axis upward, and X', Y', and Z' forming a right-handed coordinate system.

3. The method for measuring the initial position and attitude of a detector experiment according to claim 2, characterized in that, The specific steps for the second preparatory work are as follows: Step 2.1: Define the test field coordinate system and determine the coordinates of the fixed targets within it. Specifically, define the center of the test field ground as the origin of the coordinate system; define the direction from the origin eastward as the X-axis; the direction from the origin northward as the Y-axis; and the direction from the origin to the sky as the Z-axis. Select M fixed target locations within the test field and define these locations as points from (x1, y1, z1) to (x...). M ,y M ,z M M>5; Step 2.2: Before the experiment, use a total station to measure the coordinates of 5 out of the M fixed target locations; specifically: The total station is set up far from the origin of the test field coordinate system and is capable of measuring at least 5 fixed target points in the test field and 4 cooperative markers on the detector; the distance l from the total station to the fixed target position is obtained by measuring the position of the fixed target using the total station. i and the angle α between it and the horizontal plane of the test field i The difference in height between the fixed target location and the test field ground and between the total station and the test field ground is denoted as h. i If i = 1 to 4, then the coordinates (x, y, z) of the total station's measurement center in the test field coordinate system can be solved using the following system of equations:

4. The method for measuring the initial position and attitude of a detector experiment according to claim 3, characterized in that, The coordinates of the four cooperation markers obtained in step five in the test field coordinate system are defined as (x 1tc ,y 1tc ,z 1tc ) to (x 4tc ,y 4tc ,z 4tc ), and obtain a set of detector target matrices mat(to):

5. The method for measuring the initial position and attitude of a detector experiment according to claim 4, characterized in that, The sixth step describes obtaining the detector coordinates (0,0,0) in the test field coordinate system, in the following form: in, Let (0,0,0) be the coordinates of the detector origin (0,0,0) in the test field coordinate system, which needs to be solved. for R represents the origin of the detector coordinate system, and R represents the rotation relationship of the detector coordinate system about the test field coordinate system, in the form of: T represents the coordinates of the origin of the detector coordinate system in the test field coordinate system, in the form of:

6. The method for measuring the initial position and attitude of a detector experiment according to claim 5, characterized in that, The process of solving the transformation matrix R in step six includes: S1. Calculate the center point of the four points in the matrix mat(to) to obtain the matrix mat(meanto); in, S2. Calculate the center point of the four points in the matrix mat(from) to obtain mat(meanfrom); in, S3. Calculate the coordinate matrices mat(to)' and mat(from)' after eliminating the displacement effect; mat(to)'=mat(to)-mat(meanto) mat(from)'=mat(from)-mat(meanfrom) S4. Calculate the smallest unit vector matrices mat(ton) and mat(fromn) for mat(to)' and mat(from)' respectively; The calculation method for mat(ton) is: mat(ton) = mat(to)' / sto1, in, The calculation method for mat(fromn) is: mat(from) = mat(from)' / sto2. in, S5. Calculate the transformation matrix R; After obtaining mat(ton) and mat(fromn), calculate the two orthogonal bases U and V of the mat(ton)*mat(fromn) matrix; after obtaining U and V, R is obtained through R = U*S*V. T The calculation yields V. T Let V be the transpose of V, and S be the identity matrix.

7. The method for measuring the initial position and attitude of a detector experiment according to claim 6, characterized in that, The method for solving the displacement matrix T in step six is ​​as follows: The displacement matrix T is obtained by subtracting (R*mat(meanfrom)) from the mean to the mean. T ) T The calculated value represents the coordinates of the detector origin in the test field coordinate system.

8. The method for measuring the initial position and attitude of a detector experiment according to claim 6, characterized in that, The rotation relationship between the detector coordinate system and the test field coordinate system is as follows: the elements of the transformation matrix R are a matrix composed of trigonometric functions of the rotation angles Ax, Ay, and Az of the detector's X', Y', and Z' coordinate axes about the test field coordinate system X, Y, and Z, respectively. Specifically: By combining the transformation matrix R obtained from U and V with the above equation, the values ​​of Ax, Ay, and Az can be calculated, thereby obtaining the rotation angle between the detector coordinate system and the test field coordinate system, i.e., the attitude angle of the detector.

9. The method for measuring the initial position and attitude of a detector experiment according to claim 1, characterized in that, The test is repeated by changing the position of at least one of the M fixed targets, and the accuracy of the detector's initial attitude measurement is verified by repeatedly measuring multiple sets of fixed targets.

10. The method for measuring the initial position and attitude of a detector experiment according to claim 1, characterized in that, The second step involves setting a cross-shaped black and white target at the fixed target location.

Images