Active visual detector pose measurement method, medium and system

Abstract

The invention belongs to the technical field of visual navigation, and particularly relates to an active visual detector pose measurement method, medium and system, and the method comprises the following steps: definition: the optical centers of all laser range finders are coplanar, and the optical center of any laser range finder is perpendicular to the connecting line of the optical centers of two adjacent laser range finders; all the laser range finders are arranged along the axis direction of the detector; setting the optical center of any laser range finder as an original point, and establishing a detector coordinate system; determining the optical center of the laser range finder; determining three-dimensional coordinate information of the contact point; determining a normal vector of a basic landing plane; determining a landing plane coordinate system; attitude determination; determining translation; and performing adjustment optimization. Compared with the prior art, the problems that in the prior art, a detector is insufficient in complex environment sensing capacity and poor in landing navigation effect are solved, and high-robustness and high-stability detector navigation pose estimation is achieved through a low-complexity detector pose measurement method.

Description

Technical Field

[0001] The present invention belongs to the technical field of visual navigation, 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 the prerequisite for navigation landing, celestial body sampling, resource exploration and sample return. Probe navigation pose estimation is the key to completing soft landing on planetary surfaces and celestial body sampling exploration. Research on probe pose estimation systems and methods has important scientific significance and application value.

[0003] Many scholars have conducted extensive research on asteroid visual navigation, which to some extent meets the current visual navigation needs of asteroid probes. However, complex planetary landing scenarios pose huge challenges to the autonomous navigation of probes. The perception environment presents high noise characteristics such as strong radiation, electromagnetic interference, time-varying illumination, and solar light pressure, which can easily cause the failure of the probe's descent phase star catalog feature extraction and lead to probe navigation failure. In addition, asteroids have sparse textures, insufficient prior information, and irregular gravity. In order to ensure safe landing / attachment, a more robust and stable navigation attitude estimation system and method is needed.

[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 the detector 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 planet 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 direction 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 the detector coordinate system O z -X z Y z Z z ;

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

[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: Determination of the normal vector of the basic landing plane: randomly select the contact points corresponding to the three laser rangefinders to construct the basic landing plane, obtain the vector from any 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 detector 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 the 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 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.

[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 coordinates 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 the detector;

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

[0027] Each of the supporting feet 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 in the same plane.

[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 meet the needs of target pose estimation in complex environments such as those with 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 meet low power requirements.

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

[0035] Figure 1 It is a structural schematic diagram of an 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 posture calculation principle of the active visual detector posture 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 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.

[0040] In the following description, all matters not covered are 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 planet 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 direction 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 the detector coordinate system O z -Xz Y z Z z ;

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

[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: Determination of the normal vector of the basic landing plane: randomly select the contact points corresponding to the three laser rangefinders to construct the basic landing plane, obtain the vector from any 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 detector 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 the 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 the detector;

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

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

[0057] More specifically, in this embodiment:

[0058] like Figure 1As shown, the measuring system includes a detector 1, a support leg 2 and a laser rangefinder 3. A plurality of support legs 2 are arranged at the bottom of the detector 1, and the bottoms of the plurality of support legs 2 are coplanar. The detector 1 in this embodiment has four support legs 2, which are arranged in rotational symmetry. On the support legs 2, a plurality of laser rangefinders 3 are mounted one by one on the plurality of support legs 2 (one laser rangefinder 3 is mounted on each support leg 2), and the plurality of laser rangefinders 3 are kept parallel to each other and arranged along the axis direction of the detector 1 (such as 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, 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 coordinate of 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 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. The basic landing plane normal vector is determined. 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 detector coordinate system using the direction cosine method:

[0075]

[0076] in, are the unit direction vectors corresponding to the coordinate axes 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 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.

[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 meet the needs of target pose estimation in complex environments such as those with 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 meet low power requirements.

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

[0087] 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 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 the 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 the 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 the 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: based on the three-dimensional coordinate information of the optical center of the laser rangefinder (3), obtain the three-dimensional coordinate information of the contact point corresponding to each laser rangefinder (3); T4: Determination of the normal vector of the basic landing plane: randomly select the contact points corresponding to the three laser rangefinders (3) to construct the basic landing plane, obtain the vector from any 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 detector 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 the cost equation, and solve the cost equation through a nonlinear optimization algorithm to determine the optimal (R, t).

2. The method for measuring the position and posture of an active visual detector according to claim 1, characterized in that: 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 two adjacent laser rangefinders (3), Z z The axis is the cross product of two direction vectors.

3. The method for measuring the position and posture of an active visual detector according to claim 1, characterized in that: 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 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.

4. The method for measuring the position and posture of an active visual detector according to claim 1, characterized in that: 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 position and posture of an active visual detector according to claim 1, characterized in that: 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 position and posture of an active visual detector according to claim 1, characterized in that: In step T8, the nonlinear optimization algorithm includes Levenberg-Marquardt algorithm, Gauss-Newton method, gradient descent method and 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 as claimed in 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 at least three; Each of the supporting legs (2) is provided with a laser rangefinder (3).

9. The active visual detector position and 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 position and posture measurement system according to claim 8, characterized in that: The bottoms of the supporting legs (2) are arranged in the same plane.

Images