Detector pose measurement system and method

Abstract

The invention belongs to the technical field of visual navigation, and particularly relates to a detector pose measurement system and method.The detector pose measurement system is arranged on a detector and comprises a camera and a laser range finder; the camera is arranged at the bottom of the detector and faces the lower part of the detector; the laser range finder is arranged on a supporting leg of the detector and faces the lower part of the detector; the number of the supporting legs is at least three, each supporting leg is provided with a laser range finder, and the laser range finders are arranged around the camera; and laser points of the at least three laser range finders are located in the view field range of the camera. Compared with the prior art, the problems that in the prior art, a perception system for visual navigation is low in integration level and a calculation method is high in complexity are solved, the perception system with the high integration level and the calculation method with the low complexity are achieved, and soft landing of a detector on the surface of a planet can be well served.

Description

Technical Field

[0001] The present invention belongs to the technical field of visual navigation, and in particular relates to a detector posture measurement system and method. Background Art

[0002] Planetary visual navigation technology is the prerequisite for navigation landing, celestial body sampling, resource exploration and sample return. According to the flight characteristics, the probe navigation process can be divided into: Earth separation phase, interplanetary cruise phase, approach rendezvous phase, flyby exploration phase and descent landing phase. Among them, the probe navigation in the descent landing phase is the key to complete the soft landing on the planetary surface and celestial body sampling exploration. The study of the probe posture estimation in the descent landing phase has important scientific significance and application value.

[0003] At present, many scholars have carried out a lot of research work on visual navigation in the descent and landing phase, but the complex space perception environment and targets pose huge challenges to the pose estimation of the probe in the descent and landing phase. Specifically: the perception environment presents high noise characteristics such as strong radiation, electromagnetic interference, time-varying light, and solar light pressure; the perception target presents characteristics such as sparse texture, high dynamic spin, and no prior information. In particular, asteroids often have no texture, short rotation, irregular gravity, and a small safe sampling area, requiring more accurate, efficient, and robust pose estimation. In addition, due to factors such as the probe's own poor load capacity, narrow carrying space, limited energy supply capacity, and low computing performance, higher requirements are placed on the integration of the perception system and the complexity of the solution method.

[0004] Therefore, it is an inevitable trend in the development of planetary visual navigation to develop dense, compact, flexibly arranged, low-energy perception systems and to develop high-precision, efficient, and robust perception methods. Summary of the invention

[0005] The purpose of the present invention is to provide a probe posture measurement system and method in order to solve at least one of the above-mentioned problems, so as to solve the problems of low integration of the perception system and high complexity of the solution method of visual navigation in the prior art. The present scheme realizes a highly integrated perception system and a low-complexity solution method, which can well serve the soft landing of the probe on the surface of the planet.

[0006] The purpose of the present invention is achieved through the following technical solutions:

[0007] The first aspect of the present invention discloses a detector posture measurement system, wherein the detector posture measurement system is arranged on the detector, and the detector posture measurement system comprises a camera and a laser rangefinder;

[0008] The camera is arranged at the bottom of the detector and is arranged toward the bottom of the detector;

[0009] The laser rangefinder is arranged on the supporting foot of the detector and is arranged toward the bottom of the detector;

[0010] There are at least three supporting legs, each of which is provided with a laser rangefinder, and the laser rangefinder is arranged around the camera;

[0011] The laser points of at least three of the laser rangefinders are located within the field of view of the camera.

[0012] Preferably, a laser rangefinder is provided on each of the supporting legs; and / or, the laser points of all the laser rangefinders are located within the field of view of the camera.

[0013] A second aspect of the present invention discloses a detector posture measurement method, which uses the detector posture measurement system as described above;

[0014] The method comprises the following steps:

[0015] Definition: The optical center of each laser rangefinder is p t,i , the optical center of the camera is O m , the measurement point of each laser rangefinder is p c,i ;

[0016] S1: The optical center of the camera O m As the origin, construct the camera coordinate system O m -X m Y m Z m , taking any measurement point on the target as the origin, construct the target coordinate system O n -X n Y n Z n ;

[0017] S2: Calibrate the camera and obtain the camera's internal parameters;

[0018] S3: Calibrate the laser rangefinder and determine the optical center p of each laser rangefinder t,i Coordinates in the camera coordinate system;

[0019] S4: Get p through laser rangefinder c,i The distance information is obtained through the camera c,i At the camera focal length p m,i The image coordinates of

[0020] S5: Establish p m,i Imaging ray space equations in camera coordinate system;

[0021] S6: Build p c,i The constraint equations of

[0022] S7: Get p in the camera coordinate system c,i Three-dimensional information;

[0023] S8: Key point determination: randomly select 3 points c,i The three-dimensional information in the camera coordinate system constitutes C i , select p c,i The corresponding three transformed measurement points p in the target coordinate system b,i Composition B i ;

[0024] S9: Attitude determination: Get C i With B i The total rotation matrix R between

[0025] S10: Translation determination: obtaining the translation matrix t of the target coordinate system relative to the camera coordinate system;

[0026] S11: Adjustment optimization: Consider the measurement points of other laser rangefinders, construct the cost equation, and solve the cost equation through nonlinear optimization algorithm to obtain the optimal (R, t).

[0027] Preferably, in step S2, the intrinsic parameter matrix of the camera is obtained by Zhang Zhengyou calibration method.

[0028] Preferably, in step S5, an imaging light space equation is established according to a pinhole imaging model.

[0029] Preferably, in step S6, according to the prior information of the distance between the laser rangefinder and the camera, construct p c,i The constraint equations.

[0030] Preferably, step S9 includes the following steps:

[0031] S91: C i and B i Decentrating, we get C ci and B ci ;

[0032] S92: C c1 and B c1 is the origin, C c1 C c2 X is the camera coordinate system m Axis, B c1 B c2 X is the target coordinate system n Axis; align the X axes of the two coordinate systems and obtain the first rotation matrix R c and the second rotation matrix R b ;

[0033] S93: Align the Y axis and Z axis of the two coordinate systems to obtain the third rotation matrix Rm ;

[0034] S94: Get the total rotation matrix

[0035] Preferably, in step S92, the X axes of the two coordinate systems are aligned by rotating around the Y axis and the Z axis.

[0036] Preferably, in step S93, the Y axis and the Z axis of the two coordinate systems are aligned by rotating around the X axis.

[0037] Preferably, in step S11, the nonlinear optimization algorithm includes Levenberg-Marquardt algorithm, Gauss-Newton method, gradient descent method and Newton method.

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

[0039] (1) The pose solution method has low complexity and does not require high computational load processes such as iteration and singular value decomposition.

[0040] (2) The laser rangefinder can actively add laser point features that are easy to identify at the landing point, overcome the problem of low visual measurement accuracy in the depth direction, and adapt to the needs of target pose estimation in complex environments such as no prior information, no texture, and strong noise.

[0041] (3) The combined measurement method of camera and laser rangefinder can realize robust multi-dimensional pose information extraction, reduce the installation accuracy of laser rangefinder, and simplify system maintenance.

[0042] (4) The system configuration space is compact and can meet the needs of narrow, slender and other layouts with limited space. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is a structural schematic diagram of the detector posture measurement system of the present invention.

[0044] Figure 2 It is a schematic diagram of the principle of obtaining three-dimensional information of the measuring point in the detector posture measurement method of the present invention.

[0045] Figure 3 Schematic diagram of the principle of aligning the X-axis for posture determination in the detector posture measurement method of the present invention.

[0046] Figure 4 Schematic diagram of the principle of aligning the Y-axis and the Z-axis for posture determination in the detector posture measurement method of the present invention.

[0047] In the figure, 1-detector, 2-camera, 3-support leg, 4-laser rangefinder. DETAILED DESCRIPTION

[0048] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0049] For the following matters not described in detail, existing technologies can be used.

[0050] Example 1

[0051] like Figure 1 As shown, the measurement system includes a detector 1, a camera 2, a support leg 3 and a laser rangefinder 4. A plurality of support legs 3 are arranged at the bottom of the detector 1, and a plurality of laser rangefinders 4 are mounted on the plurality of support legs 3 in a one-to-one correspondence (only one laser rangefinder 4 is mounted on each support leg 3); the camera 2 is mounted at the bottom of the detector 1, and a plurality of laser rangefinders 4 surround the camera 2.

[0052] The camera 2 and the laser rangefinder 4 are both arranged toward the bottom of the detector 1 , and the laser point of the laser rangefinder 4 is within the field of view of the camera 2 .

[0053] Example 2

[0054] like Figure 2-4 As shown, the measurement method is as follows:

[0055] System initialization. Initialize multiple laser rangefinders 4 and cameras 2, and define the optical center of each laser rangefinder 4 as p t,i , the optical center of camera 2 is O m Each laser rangefinder has 4 measurement points p c,i ;

[0056] S1. Establish a coordinate system. Take the optical center of camera 2 as m As the origin, construct the camera coordinate system O m -X m Y m Z m , taking any measurement point on the target (planetary surface) as the origin, construct the target coordinate system O n -X n Y n Z n ;

[0057] S2. Calibrate camera 2. Use Zhang Zhengyou calibration method to determine the focal length f of camera 2 c , principal points and distortion coefficients;

[0058] S3. Calibrate the laser rangefinder 4. Determine the optical center p of each laser rangefinder 4 t,i Coordinates in the camera coordinate system;

[0059] S4. Information acquisition. The laser rangefinder 4 acquires the measurement point pc,i The distance information of camera 2 is captured by p c,i The image coordinates p m,i (u,v)(i.e., p c,i With O m The intersection of the line at the focal length of camera 2);

[0060] S5. Establish the imaging light equation. Figure 2 As shown, according to the pinhole imaging model, it is determined that m -X m Y m Z m Next m,i (u,v,f c ) Imaging light space equation:

[0061]

[0062] Among them, f c is the focal length of camera 2, (u,v) is p c,i The image coordinates (i.e., p m,i (u,v));

[0063] S6. Construct measurement point p c,i The constraint equations are as follows. Figure 2 As shown, based on the prior information of the distance between the laser rangefinder 4 and the camera 2, the measurement point p is constructed. c,i The constraint equations are:

[0064]

[0065] Among them, (a c ,b c ,c c ) is p t,i In O m -X m Y m Z m The coordinates under (x, y, z) are p c,i The three-dimensional information, d c is the measured distance of the laser rangefinder 4;

[0066] S7. Determine the measuring point p c,i 3D information in the camera coordinate system. Solve the constraint equations in step S6 to obtain each measurement point p c,i Three-dimensional information:

[0067]

[0068] According to the actual position of the point, i.e. z must be greater than 0, other multiple solutions are discarded;

[0069] S8. Determine the key points. Randomly select 3 p c,i The three-dimensional information in the camera coordinate system constitutes C i , select p c,i The corresponding three transformed measurement points p in the target coordinate system b,i Composition B i ;

[0070] S9. Posture determination. First, C i and B i De-centroiding, that is

[0071]

[0072] C c1 and B c1 is the origin, C c1 C c2 X is the camera coordinate system m Axis, B c1 B c2 X is the target coordinate system n Axis, align the X axes of the two coordinate systems, that is, rotate around the Y and Z axes to make the X axes of the two coordinate systems consistent (coincident), such as Figure 3 shown.

[0073]

[0074] Where a and b are the camera coordinate system around Y m The sine and cosine values ​​of the axis rotation angle, c and d are the values ​​of the rotation angle around Z m The sine and cosine values ​​of the axis rotation angle, e and f are the target coordinate system around Y n The sine and cosine values ​​of the rotation angle of the axis, g and h are the values ​​around Z n Sine and cosine of the shaft rotation angle, R c and R b The rotation matrix to align the two X axes.

[0075] Just C c2 For , we can get the constraint equation:

[0076]

[0077] Combined with the characteristics of the rotation matrix, we can get b=-C c2z / ||C c2 ||,c=C c2x / (a||C c2 ||), d = C c2y / (a||C c2 ||), further calculation can get R c , and R b .like Figure 4 As shown, finally the Y and Z axes are aligned by rotating the X axis, that is:

[0078]

[0079] In the formula, , k and p are the sine and cosine of the rotation angle.

[0080] The constraint equation is:

[0081]

[0082] Available p=(kN 3y -M 3y ) / N 3z .

[0083] The rotation matrix can be obtained

[0084] S10. Translation determination. The translation matrix t of the target coordinate system relative to the camera coordinate system, that is, the coordinates of the origin of the target coordinate system in the camera coordinate system:

[0085] t=C c1 -RB c1

[0086] S11. Adjustment optimization. Taking into account the measurement points of other laser rangefinders 4, the cost equation is constructed:

[0087]

[0088] The cost equation is solved by using the Levenberg-Marquardt algorithm to determine the optimal (R, t). Among other methods, nonlinear optimization algorithms such as the Gauss-Newton method, the gradient descent method, and the Newton method can also be used to solve the cost equation.

[0089] In summary, the present system and method:

[0090] (1) The pose solution method has low complexity and does not require high computational load processes such as iteration and singular value decomposition.

[0091] (2) The laser rangefinder 4 can actively add laser point features that are easy to identify at the landing point, overcome the problem of low visual measurement accuracy in the depth direction, and adapt to the needs of target pose estimation in complex environments such as no prior information, no texture, and strong noise.

[0092] (3) The combined measurement method of the camera 2 and the laser rangefinder 4 can realize robust multi-dimensional pose information extraction, reduce the installation accuracy of the laser rangefinder 4, and simplify system maintenance.

[0093] (4) The system configuration space is compact and can meet the needs of narrow, slender and other layouts with limited space.

[0094] The above description of the embodiments is to facilitate the understanding and use of the invention by those skilled in the art. It is obvious that those skilled in the art can easily make various modifications to these embodiments and apply the general principles described herein to other embodiments without creative work. Therefore, the present invention is not limited to the above embodiments, and improvements and modifications made by those skilled in the art based on the disclosure of the present invention without departing from the scope of the present invention should be within the scope of protection of the present invention.

Claims

1. A detector posture measurement system, the detector posture measurement system being arranged on a detector (1), characterized in that: The detector posture measurement system comprises a camera (2) and a laser rangefinder (4); The camera (2) is arranged at the bottom of the detector (1) and is arranged toward the bottom of the detector (1); The laser rangefinder (4) is arranged on the supporting foot (3) of the detector (1) and is arranged toward the bottom of the detector (1); At least three supporting legs (3) are provided, each supporting leg (3) is provided with a laser rangefinder (4), and the laser rangefinder (4) is arranged around the camera (2); The laser points of at least three of the laser rangefinders (4) are located within the field of view of the camera (2).

2. A detector posture measurement system according to claim 1, characterized in that: A laser rangefinder (4) is arranged on each of the supporting legs (3); and / or the laser points of all the laser rangefinders (4) are located within the field of view of the camera (2).

3. A detector posture measurement method, characterized in that: Using the detector posture measurement system as claimed in claim 1 or 2; The method comprises the following steps: Definition: The optical center of each laser rangefinder (4) is p t,i , the optical center of camera (2) is O m , the measurement point of each laser rangefinder (4) is p c,i ; S1: The optical center O of the camera (2) m As the origin, construct the camera coordinate system O m -X m Y m Z m , taking any measurement point on the target as the origin, construct the target coordinate system O n -X n Y n Z n ; S2: calibrate camera (2) and obtain the intrinsic parameters of camera (2); S3: Calibrate the laser rangefinder (4) and determine the optical center p of each laser rangefinder (4) t,i Coordinates in the camera coordinate system; S4: Obtain p through the laser rangefinder (4) c,i The distance information is obtained through the camera (2) c,i At the focal length p of the camera (2) m,i The image coordinates of S5: Establish p m,i Imaging ray space equations in camera coordinate system; S6: Build p c,i The constraint equations of S7: Get p in the camera coordinate system c,i Three-dimensional information; S8: Key point determination: randomly select 3 points c,i The three-dimensional information in the camera coordinate system constitutes C i , select p c,i The corresponding three transformed measurement points p in the target coordinate system b,i Composition B i ; S9: Attitude determination: Get C i With B i The total rotation matrix R between S10: Translation determination: obtaining the translation matrix t of the target coordinate system relative to the camera coordinate system; S11: Adjustment optimization: Consider the measurement points of other laser rangefinders (4), construct a cost equation, and solve the cost equation through a nonlinear optimization algorithm to obtain the optimal (R, t).

4. A detector posture measurement method according to claim 3, characterized in that: In step S2, the intrinsic parameter matrix of the camera (2) is obtained by the Zhang Zhengyou calibration method.

5. A detector posture measurement method according to claim 3, characterized in that: In step S5, an imaging light space equation is established according to the pinhole imaging model.

6. A detector posture measurement method according to claim 3, characterized in that: In step S6, based on the prior information of the distance between the laser rangefinder (4) and the camera (2), construct p c,i The constraint equations.

7. A detector posture measurement method according to claim 3, characterized in that: Step S9 includes the following steps: S91: C i and B i Decentrating, we get C ci and B ci ; S92: C c1 and B c1 is the origin, C c1 C c2 X is the camera coordinate system m Axis, B c1 B c2 X is the target coordinate system n Axis; align the X axes of the two coordinate systems and obtain the first rotation matrix R c and the second rotation matrix R b ; S93: Align the Y axis and Z axis of the two coordinate systems to obtain the third rotation matrix R m ; S94: Obtain the total rotation matrix R = R c T R m R b .

8. A detector posture measurement method according to claim 7, characterized in that: In step S92, the X axes of the two coordinate systems are aligned by rotating around the Y axis and the Z axis.

9. A detector posture measurement method according to claim 7, characterized in that: In step S93, the Y axis and the Z axis of the two coordinate systems are aligned by rotating around the X axis.

10. A detector posture measurement method according to claim 3, characterized in that: In step S11, the nonlinear optimization algorithm includes Levenberg-Marquardt algorithm, Gauss-Newton method, gradient descent method and Newton method.

Images