Active vision detector posture measurement method, medium and system

Abstract

The present invention belongs to the field of visual navigation technology, and specifically relates to an active visual detector posture measurement method, medium, and system, comprising the following steps: defining that the optical centers of all laser rangefinders are coplanar, and that the optical center of any laser rangefinder is perpendicular to the line connecting the optical centers of two adjacent laser rangefinders; all laser rangefinders are arranged along the axis of the detector; setting the optical center of any laser rangefinder as the origin and establishing a detector coordinate system; determining the optical center of the laser rangefinder; determining the three-dimensional coordinate information of the contact point; determining the normal vector of the basic landing plane; determining the landing plane coordinate system; determining the posture; determining the translation; and optimizing the adjustment. Compared with the prior art, the present invention solves the problems of the prior art in the detector's insufficient perception of complex environments and poor landing navigation performance. This solution achieves highly robust and stable detector navigation posture estimation through a low-complexity detector posture measurement method.

Description

Technical Field

[0001] The present invention belongs to the field of visual navigation technology, and in particular relates to a method, medium and system for measuring the posture of an active visual detector. Background Art

[0002] Planetary visual navigation technology is a prerequisite for navigational landing, celestial body sampling, resource exploration, and sample return. Probe navigation pose estimation is crucial for soft landing on planetary surfaces and celestial body sampling and exploration. Research on probe pose estimation systems and methods has important scientific significance and application value.

[0003] Numerous researchers have conducted extensive research on visual navigation of asteroids, which, to a certain extent, meets the current visual navigation requirements for asteroid probe landings. However, the complex planetary landing scenarios present significant challenges for autonomous probe navigation. The sensing environment is characterized by high noise levels, such as strong radiation, electromagnetic interference, time-varying illumination, and solar pressure. These factors can easily lead to ineffective star catalog feature extraction during the probe's descent, resulting in navigation failures. Furthermore, asteroids exhibit sparse textures, insufficient prior information, and irregular gravity. To ensure safe landing and attachment, robust and stable navigation pose estimation systems and methods are essential.

[0004] Based on this, it is necessary to propose a method for estimating the navigation pose of a detector that is highly robust and stable. Summary of the Invention

[0005] The purpose of the present invention is to provide an active visual detector posture measurement method, medium and system in order to solve at least one of the above problems, so as to solve the problems of insufficient perception of complex environments and poor landing navigation effect of detectors in the prior art. This scheme realizes highly robust and stable detector navigation posture estimation through a low-complexity detector posture measurement method.

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

[0007] The first aspect of the present invention discloses a method for measuring the posture of an active visual detector, comprising the following steps:

[0008] Definition: The optical center of each laser rangefinder is p t,i , the contact point between each laser rangefinder and the planetary surface is p z,i , the distance from each laser rangefinder to the corresponding contact point is d i ;

[0009] The optical centers of all laser rangefinders are arranged in the same plane, and the optical center of any laser rangefinder is perpendicular to the line connecting the optical centers of two adjacent laser rangefinders; all laser rangefinders are arranged along the axis of the detector;

[0010] T1: Set the optical center of any laser rangefinder as the origin O z , calibrate the optical center of the remaining laser rangefinders to the origin O z distance, and based on the origin O z Establish detector coordinate system O z -X z Y z Z z ;

[0011] T2: Determine the optical center of the laser rangefinder: Based on the prior information between the optical centers of each laser rangefinder, obtain the three-dimensional coordinate information of the optical center of each laser rangefinder;

[0012] T3: Determine the three-dimensional coordinate information of the contact point: Obtain the three-dimensional coordinate information of the contact point corresponding to each laser rangefinder based on the three-dimensional coordinate information of the optical center of the laser rangefinder;

[0013] T4: Determine the normal vector of the basic landing plane: Construct the basic landing plane using any three contact points corresponding to the laser rangefinders. Obtain the vector from any one point to the other two points, and calculate the normal vector of the basic landing plane by cross-producting the two vectors.

[0014] T5: Determine the landing plane coordinate system: Establish the landing plane coordinate system and obtain the direction vector of each coordinate axis;

[0015] T6: Attitude determination: Determine the attitude matrix R of the landing plane coordinate system relative to the probe coordinate system;

[0016] T7: Translation determination: Determine the translation matrix t of the landing plane coordinate system relative to the probe coordinate system;

[0017] T8: Adjustment optimization: Consider other landing contact points, construct a cost equation, and solve the cost equation through a nonlinear optimization algorithm to determine the optimal (R, t).

[0018] Preferably, in step T2, the detector coordinate system O z -X z Y z Z z Based on the origin O z Establish, where X z Axis and Y z The axes are respectively the origin O z The direction vector from the optical center of the corresponding laser rangefinder to the optical centers of the two adjacent laser rangefinders, Z z The axis is the cross product of two direction vectors.

[0019] Preferably, in step T5, the landing plane coordinate system is based on the origin O zThe contact point corresponding to the corresponding laser rangefinder is the origin of the landing plane coordinate system, the basic landing plane normal vector is the Z axis of the landing plane coordinate system, the direction vector of the line connecting the origin of the landing plane coordinate system to any contact point is the Y axis of the landing plane coordinate system, and the cross product of the basic landing plane normal vector and the direction vector of the line connecting the origin of the landing plane coordinate system to any contact point is the X axis.

[0020] Preferably, in step T6, the attitude matrix R of the landing plane coordinate system relative to the detector coordinate system is determined according to the direction cosine method.

[0021] Preferably, in step T7, the translation matrix t is the coordinate of the origin of the landing plane coordinate system in the detector coordinate system.

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

[0023] A second aspect of the present invention discloses a computer-readable storage medium, wherein the computer-readable storage medium includes a stored computer program, wherein when the computer program is running, the device where the computer-readable storage medium is located is controlled to execute any of the above methods.

[0024] A third aspect of the present invention discloses an active visual detector posture measurement system, which adopts any of the above methods;

[0025] The system is mounted on a detector;

[0026] The system includes a laser rangefinder, which is arranged on the supporting legs of the detector, and the supporting legs are no less than three;

[0027] Each of the supporting legs is provided with a laser rangefinder.

[0028] Preferably, four laser rangefinders are provided, and the optical centers of all the laser rangefinders are located at the four corners of the same rectangle.

[0029] Preferably, the bottoms of the supporting legs are arranged coplanarly.

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

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

[0032] (2) Actively add easily identifiable features through the laser rangefinder to adapt to the target pose estimation needs in complex environments such as no prior information, no texture, and strong noise.

[0033] (3) Only a single sensing system, the laser rangefinder, can realize multi-dimensional information extraction. The system is simple and reliable and can adapt to low power requirements.

[0034] (4) The system configuration space is compact and adapts to the limited layout space requirements of the detector. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 Schematic diagram of the structure of the active visual detector posture measurement system in an embodiment of the present invention;

[0036] Figure 2 Schematic diagram of the posture measurement principle of the active visual detector posture measurement method in an embodiment of the present invention;

[0037] Figure 3 Schematic diagram of the pose calculation principle of the active visual detector pose measurement method in an embodiment of the present invention;

[0038] In the figure: 1-detector; 2-support leg; 3-laser rangefinder. DETAILED DESCRIPTION

[0039] The present invention is described in detail below with reference to 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.

[0040] In the following description, matters not covered are all prior art.

[0041] Example

[0042] An active visual detector pose measurement method, such as Figure 2 、 3 As shown, the following steps are included:

[0043] Definition: The optical center of each laser rangefinder is p t,i , the contact point between each laser rangefinder and the planetary surface is p z,i , the distance from each laser rangefinder to the corresponding contact point is d i ;

[0044] The optical centers of all laser rangefinders are arranged in the same plane, and the optical center of any laser rangefinder is perpendicular to the line connecting the optical centers of two adjacent laser rangefinders; all laser rangefinders are arranged along the axis of the detector;

[0045] T1: Set the optical center of any laser rangefinder as the origin O z , calibrate the optical center of the remaining laser rangefinders to the origin O z distance, and based on the origin O z Establish detector coordinate system O z -Xz Y z Z z ;

[0046] T2: Determine the optical center of the laser rangefinder: Based on the prior information between the optical centers of each laser rangefinder, obtain the three-dimensional coordinate information of the optical center of each laser rangefinder;

[0047] T3: Determine the three-dimensional coordinate information of the contact point: Obtain the three-dimensional coordinate information of the contact point corresponding to each laser rangefinder based on the three-dimensional coordinate information of the optical center of the laser rangefinder;

[0048] T4: Determine the normal vector of the basic landing plane: Construct the basic landing plane using any three contact points corresponding to the laser rangefinders. Obtain the vector from any one point to the other two points, and calculate the normal vector of the basic landing plane by cross-producting the two vectors.

[0049] T5: Determine the landing plane coordinate system: Establish the landing plane coordinate system and obtain the direction vector of each coordinate axis;

[0050] T6: Attitude determination: Determine the attitude matrix R of the landing plane coordinate system relative to the probe coordinate system;

[0051] T7: Translation determination: Determine the translation matrix t of the landing plane coordinate system relative to the probe coordinate system;

[0052] T8: Adjustment optimization: Consider other landing contact points, construct a cost equation, and solve the cost equation through a nonlinear optimization algorithm to determine the optimal (R, t).

[0053] An active visual detector pose measurement system, such as Figure 1 As shown, the method described above is adopted;

[0054] The system is mounted on a detector;

[0055] The system includes a laser rangefinder, which is arranged on the supporting legs of the detector, and the supporting legs are no less than three;

[0056] Each of the supporting legs is provided with a laser rangefinder.

[0057] More specifically, in this embodiment:

[0058] like Figure 1As shown, the measurement system includes a detector 1, support legs 2, and laser rangefinders 3. Multiple support legs 2 are arranged at the bottom of the detector 1, and the bottoms of the multiple support legs 2 are coplanar. The detector 1 in this embodiment has four support legs 2, which are arranged in rotational symmetry. On them, multiple laser rangefinders 3 are installed on the multiple support legs 2 in a one-to-one correspondence (each support leg 2 is equipped with a laser rangefinder 3), and the multiple laser rangefinders 3 are kept parallel to each other and arranged along the axis direction of the detector 1 (as shown in FIG. Figure 1 As shown, it is arranged vertically downward).

[0059] In addition, the optical centers of the plurality of laser rangefinders 3 are arranged in the same plane, and the optical centers of the four laser rangefinders 3 in this embodiment are arranged in a rectangular shape (distributed at the four corners of the rectangle).

[0060] The measurement method is as follows:

[0061] S1. Initialize multiple laser rangefinders and define the optical center of each laser rangefinder as p t,i , setting the optical centers of multiple laser rangefinders to be coplanar;

[0062] S2. Optical centers p of four laser rangefinders t,1 ~p t,4 Arranged in a rectangular shape, and vertical, and vertical, and vertical, and vertical;

[0063] S3. Take the optical center of the second laser rangefinder as the origin O z , calibrate the distance between the optical center of each laser rangefinder and the optical center of the second laser rangefinder;

[0064] S4. is the detector coordinate system X z Axis, with Y is the detector coordinate system z axis, then is the detector coordinate system Z z Axis, establish the detector coordinate system O z -X z Y z Z z ;

[0065] S5. Determine the optical center of the laser rangefinder. Figure 2 As shown, combined with the geometric information of each laser rangefinder in the detector coordinate system (prior information), the three-dimensional information of the optical center of each laser rangefinder in the detector coordinate system is determined:

[0066]

[0067] Where a is the first laser rangefinder in the detector coordinate system X z The installation distance in the axis direction, b is the third laser rangefinder in the detector coordinate system Y z Installation distance in the axial direction;

[0068] S6. Determine the three-dimensional information of the landing contact point. Figure 2 As shown, the distance d between each contact point and the optical center of the laser rangefinder is obtained according to each laser rangefinder. i , combined with the geometric information of each laser rangefinder in the detector coordinate system, determine the three-dimensional information of each contact point in the detector coordinate system:

[0069]

[0070] S7. Determine the normal vector of the basic landing plane. Figure 2 As shown, select the contact points corresponding to the 1st, 2nd and 3rd laser rangefinders to construct the basic landing plane and determine Calculate the base landing plane normal vector:

[0071]

[0072] S8. Determine the landing plane coordinate system. Figure 3 As shown, take the contact point corresponding to the second laser rangefinder as the origin and set is the Z axis of the landing plane coordinate system, is the Y axis of the landing plane coordinate system, is the X-axis of the landing plane coordinate system. The direction vectors of each coordinate axis of the landing plane coordinate system are:

[0073]

[0074] S9. Attitude determination. Determine the attitude matrix R of the landing plane coordinate system relative to the probe coordinate system using the direction cosine method:

[0075]

[0076] in, are the unit direction vectors corresponding to each coordinate axis of the detector coordinate system, and each element value in the direction cosine matrix is ​​the cosine value of the angle between two unit direction vectors;

[0077] S10. Translation determination. The translation matrix t of the landing plane coordinate system relative to the detector coordinate system, that is, the coordinates of the origin of the landing plane coordinate system in the detector coordinate system:

[0078] t=(0,0,-d2);

[0079] S11. Adjustment optimization. Taking into account other landing contact points, construct the cost equation:

[0080]

[0081] The cost equation is solved using the Levenberg-Marquardt algorithm to determine the optimal (R, t). Other methods that can be used to solve the cost equation include nonlinear optimization algorithms such as the Gauss-Newton method, gradient descent, and Newton's method.

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

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

[0084] (2) Actively add easily identifiable features through the laser rangefinder to adapt to the target pose estimation needs in complex environments such as no prior information, no texture, and strong noise.

[0085] (3) Only a single sensing system, the laser rangefinder, can realize multi-dimensional information extraction. The system is simple and reliable and can adapt to low power requirements.

[0086] (4) The system configuration space is compact and adapts to the limited layout space requirements of the detector.

[0087] The above description of the embodiments is intended to facilitate understanding and use of the invention by those skilled in the art. It will be apparent that those skilled in the art can readily make various modifications to these embodiments and apply the general principles described herein to other embodiments without requiring inventive effort. Therefore, the present invention is not limited to the above-described embodiments. 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 method for measuring the posture of an active visual detector, characterized in that: The steps include: Definition: The optical center of each laser rangefinder (3) is p t,i , the contact point between each laser rangefinder (3) and the planetary surface is p z,i , the distance from each laser rangefinder (3) to the corresponding contact point is d i ; The optical centers of all laser rangefinders (3) are arranged in the same plane, and the optical center of any laser rangefinder (3) is perpendicular to the line connecting the optical centers of two adjacent laser rangefinders (3); all laser rangefinders (3) are arranged along the axis direction of the detector (1); T1: Set the optical center of any laser rangefinder (3) as the origin O z , calibrate the optical center of the remaining laser rangefinders (3) to the origin O z distance, and based on the origin O z Establish detector coordinate system O z -X z Y z Z z ; T2: Determine the optical center of the laser rangefinder (3): Based on the prior information between the optical centers of each laser rangefinder (3), obtain the three-dimensional coordinate information of the optical center of each laser rangefinder (3); T3: Determine the three-dimensional coordinate information of the contact point: obtain the three-dimensional coordinate information of the contact point corresponding to each laser rangefinder (3) based on the three-dimensional coordinate information of the optical center of the laser rangefinder (3); T4: Determination of the normal vector of the basic landing plane: Take any three contact points corresponding to the laser rangefinder (3) to construct the basic landing plane, obtain the vector from any one point to the other two points, and calculate the normal vector of the basic landing plane by cross-producting the two vectors; T5: Determine the landing plane coordinate system: Establish the landing plane coordinate system and obtain the direction vector of each coordinate axis; T6: Attitude determination: Determine the attitude matrix R of the landing plane coordinate system relative to the probe coordinate system; T7: Translation determination: Determine the translation matrix t of the landing plane coordinate system relative to the probe coordinate system; T8: Adjustment optimization: Consider other landing contact points, construct a cost equation, and solve the cost equation through a nonlinear optimization algorithm to determine the optimal (R, t).

2. The method for measuring the posture of an active visual detector according to claim 1, wherein: In step T2, the detector coordinate system O z -X z Y z Z z Based on the origin O z Establish, where X z Axis and Y z The axes are respectively the origin O z The direction vector from the optical center of the corresponding laser rangefinder (3) to the optical centers of the two adjacent laser rangefinders (3), Z z The axis is the cross product of two direction vectors.

3. The method for measuring the posture of an active visual detector according to claim 1, wherein: In step T5, the landing plane coordinate system is based on the origin O z The contact point corresponding to the corresponding laser rangefinder (3) is the origin of the landing plane coordinate system, the basic landing plane normal vector is the Z axis of the landing plane coordinate system, the direction vector of the line from the origin of the landing plane coordinate system to any contact point is the Y axis of the landing plane coordinate system, and the cross product of the basic landing plane normal vector and the direction vector of the line from the origin of the landing plane coordinate system to any contact point is the X axis.

4. The method for measuring the posture of an active visual detector according to claim 1, wherein: In step T6, the attitude matrix R of the landing plane coordinate system relative to the detector coordinate system is determined according to the direction cosine method.

5. The method for measuring the posture of an active visual detector according to claim 1, wherein: In step T7, the translation matrix t is the coordinate of the origin of the landing plane coordinate system in the detector coordinate system.

6. The method for measuring the posture of an active visual detector according to claim 1, wherein: In step T8, the nonlinear optimization algorithm includes the Levenberg-Marquardt algorithm, the Gauss-Newton method, the gradient descent method and the Newton method.

7. A computer-readable storage medium, characterized in that The computer-readable storage medium includes a stored computer program, wherein when the computer program is running, the device where the computer-readable storage medium is located is controlled to execute the method according to any one of claims 1 to 6.

8. An active visual detector posture measurement system, characterized in that: Adopting the method according to any one of claims 1 to 6; The system is mounted on a detector (1); The system comprises a laser rangefinder (3), wherein the laser rangefinder (3) is arranged on a supporting leg (2) of the detector (1), and the supporting leg (2) is not less than three; Each of the supporting legs (2) is provided with a laser rangefinder (3).

9. The active visual detector posture measurement system according to claim 8, characterized in that: Four laser rangefinders (3) are provided, and the optical centers of all the laser rangefinders (3) are located at the four corners of the same rectangle.

10. The active visual detector posture measurement system according to claim 8, characterized in that: The bottoms of the supporting legs (2) are arranged in a coplanar manner.

Images