A method for estimating the rotation parameters of small celestial bodies by integrating cameras and lidar

Abstract

The present invention discloses a method for estimating the rotation parameters of small celestial bodies by integrating a camera and a lidar, comprising the following steps: defining a coordinate system, establishing a measurement model for integrating the camera and the lidar, defining an observation block matrix, and fusing various observation information through the block matrix to realize the estimation of the rotation parameters of small celestial bodies. The present invention improves the estimation accuracy of the spin parameters of small celestial bodies by introducing the depth information of feature points, and simultaneously calculates the position and velocity of the small celestial body relative to the detector.

Description

Technical Field

[0001] The present invention belongs to the technical field of deep space exploration and is applicable to the method for estimating the rotation parameters of small celestial bodies in a dim environment. Background Technique

[0002] In order to enable the detector to safely reach the target celestial body, it is necessary to determine the spin angular velocity, the direction of the spin axis, and the motion state of the small celestial body. Therefore, it is very necessary to study a stable and accurate method for estimating the motion state of small celestial bodies.

[0003] During the landing process of the detector, image and distance information are important sources for obtaining the rotation parameters of small celestial bodies. Measuring with a camera has low cost, high resolution, and wide range, and there are already many proven methods for estimating the rotation parameters of small celestial bodies. However, the camera has high requirements for lighting conditions and it is difficult to robustly estimate the motion state of small celestial bodies in a complex deep space environment. 3D point cloud, as a kind of distance information, can be obtained by lidar and is insensitive to lighting conditions. However, the resolution of the point cloud collected by it is low, which will affect the accuracy of point cloud registration. Summary of the Invention

[0004] Aiming at the above problems, the present invention proposes a method for estimating the rotation parameters of small celestial bodies by fusing a camera and a lidar. The present invention fuses a camera and a lidar, which can effectively overcome the disadvantages of the two and thus realize the robust estimation of the motion state of small celestial bodies.

[0005] To achieve the above object, the present invention adopts the following technical solutions:

[0006] The method for estimating the rotation parameters of small celestial bodies in a dim environment disclosed by the present invention installs a lidar and a monocular camera on the lander. A measurement model that fuses the camera and the lidar is established, and by tracking the image feature points with depth information, the spin angular velocity, the direction of the spin axis, the position, and the velocity of the small celestial body are estimated using an extended Kalman filter.

[0007] A method for estimating the rotation parameters of small celestial bodies by fusing a camera and a lidar disclosed by the present invention includes the following steps: S1: Define a coordinate system, establish a relative motion model of the small celestial body and the detector, and establish a state equation; use the image feature points and their depth information as the observation quantities of the filter, and design a joint camera and lidar observation equation.

[0008] S2: Define an observation block matrix, fuse various observation information through the block matrix, and realize the estimation of the rotation parameters of the small celestial body.

[0009] By adopting the above technical solution, the beneficial effects of the present invention are as follows: The texture of the images captured by the camera is relatively rich, but it has high requirements for environmental lighting conditions. The lidar obtains distance information and is not affected by conditional light, but has a low resolution, which affects the accuracy of point cloud registration. Fusing the camera and the lidar can effectively overcome the disadvantages of both and thus achieve a robust estimation of the motion state of small celestial bodies. It should be noted that according to the depth information of the image feature points, the algorithm in this paper can also estimate the relative pose of the detector and the small celestial body simultaneously. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 It is a flowchart of the method for estimating the motion state of small celestial bodies by fusing a camera and a lidar according to an embodiment of the present invention;

[0011] Figure 2 It is a schematic diagram of coordinate system definition in an embodiment of the present invention;

[0012] Figure 3 It is a schematic diagram of the filtering algorithm in an embodiment of the present invention;

[0013] Figure 4 It is a diagram of the estimation result of the motion state of small celestial bodies in an embodiment of the present invention;

[0014] Figure 4 (a) is a diagram of the estimation result of the rotational angular velocity of the small celestial body; (b) is the estimation result of the rotation axis of the small celestial body; (c) is a diagram of the estimation result of the position of the small celestial body; (d) is a diagram of the estimation result of the position of the small celestial body. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0015] In order to better illustrate the purpose and advantages of the present invention, the content of the present invention will be described more clearly and completely with reference to the accompanying drawings.

[0016] As Figure 1 shown, a method for physical parameters of small celestial bodies by fusing a camera and a lidar disclosed in this embodiment is specifically implemented as follows:

[0017] Step S1: Establish a relative motion model and a measurement model. First, determine the coordinate system, then establish a relative motion model of the small celestial body and the detector, and finally establish a measurement model of the camera and the lidar. Step S2: Define an observation block matrix, and fuse various observation information through the block matrix to realize the estimation of the rotation parameters of the small celestial body.

[0018] Furthermore, the coordinate system definition in step S1 includes the following steps: In order to facilitate the description of the relative pose of the detector and the small celestial body, three right-handed coordinate systems are introduced: The camera coordinate system takes the optical center of the camera as the origin, assuming that the radar coordinate system coincides with the camera, and the centroid coordinate system of the small celestial body is used to describe the position of the feature points. The coordinate system relationship is as Figure 2 shown.

[0019] The camera coordinate system OC -X C Y C Z C : Let the origin O of this coordinate system C be located at the centroid of the detector, and the Z-axis direction points from the optical center of the camera to the centroid of the small celestial body. The coordinate system X C , Y C , Z C axes satisfy the right-hand coordinate system rule. This coordinate system is used to observe the motion state of the small celestial body. The radar coordinate system O L -X L Y L Z L :: Assume that the radar coordinate system coincides with the camera coordinate system, and the small celestial body centroid coordinate system O B -X B Y B Z B : Assume that its origin O B is located at the centroid position of the target celestial body. The coordinate system X B , Y B , Z B axes satisfy the right-hand coordinate system rule. This coordinate system is used to describe the spatial position of the observed feature points.

[0020] Furthermore, the coordinate system definition in step S1 includes the following steps: To facilitate the description of the relative pose between the detector and the small celestial body, three right-hand coordinate systems are introduced: The camera coordinate system takes the optical center of the camera as the origin. Assume that the radar coordinate system coincides with the camera. The small celestial body centroid coordinate system is used to describe the position of the feature points.

[0021] Establish a relative motion model; including the following steps:

[0022] Define the filtering state vector as consisting of the position, velocity, attitude quaternion, spin angular velocity of the small celestial body, and the position of the feature point, and the specific definition is as follows:

[0023] s = [r v q B / C ω x p (1)

[0024] In the formula, r and v are the position and motion velocity of the detector relative to the small celestial body in the coordinate system O C -X C Y C Z C ; q B / C is the attitude quaternion of the centroid of the small celestial body relative to the centroid of the detector; ω represents the angular velocity vector of the three axes of the small celestial body coordinate; the spatial coordinates of the image feature point in the small celestial body centroid coordinate system O B -X B Y B Z B are represented as x p .

[0025] In the camera coordinate system, the differential of the position vector of the small celestial body is its velocity, assuming that the differentials of its velocity and rotational angular velocity are zero; the small celestial body is a rigid body, and the coordinates of the tracked feature point in the centroid coordinate system of the small celestial body remain unchanged.

[0026]

[0027] Wherein,

[0028] During the landing process of the detector, the monocular camera acquires navigation images, and image feature points are obtained through the SURF algorithm. The lidar acquires the depth information of the feature points, projects the laser point cloud onto the image plane, and establishes the matching relationship between the image feature points and the depth information of the feature points. Assume that the measurement of the i-th feature point is

[0029]

[0030] Wherein, [u i v i ] is the pixel coordinate of the feature point, is the three-dimensional coordinate of the feature point in the camera coordinate system, and f is the focal length of the camera.

[0031] The lidar measurement of the j-th feature point is

[0032]

[0033] Wherein, γ j is the distance of the j-th feature point relative to the detector, r B is the position vector of the detector relative to the centroid of the small celestial body; is the position vector of the feature point relative to the centroid of the small celestial body.

[0034] The position coordinates of the detector at time t in the camera coordinate system are defined as

[0035] r C (t) = [x(t) y(t) z(t)] T (5)

[0036] The coordinates of the i-th feature point in the centroid coordinate system of the small celestial body are expressed as

[0037]

[0038] The i-th feature point in the camera coordinate system O C -X C Y C Z C can be expressed as

[0039] P iC r(t) = C r(t) + R B / C P(t) i B (t)(7)

[0040] The position of the detector at time t in the centroid coordinate system of the small celestial body is

[0041]

[0042] In the centroid coordinate system of the small celestial body, the vector between the camera and the i-th feature point is

[0043]

[0044] According to equations (3) and (7), the observation equation of the camera is shown in formula (10)

[0045]

[0046] According to equation (9), the observation equation of the lidar is shown in formula (11)

[0047]

[0048] Furthermore, in step S1, the state transition equation is written in the standard extended Kalman filter form; including the following steps:

[0049]

[0050]

[0051] The Jacobian matrix of the state transition matrix is:

[0052]

[0053]

[0054] Furthermore, in step S2, the observation block matrix is defined, and multiple observation information is fused through the block matrix; including the following steps:

[0055] There are three cases for whether the image feature points and the 3D point cloud are aligned:

[0056] X = [X0 X1 X2 X3] (16)

[0057] Where X0 is the position, velocity, attitude quaternion, and angular velocity of the small celestial body, X1 is the coordinate of the successfully fused feature point, X2 is the coordinate of the image feature point, and X3 is the coordinate of the 3D point cloud.

[0058] (1) According to equations (10) and (11), the observation equation of the successfully aligned depth image feature point information is expressed as h cl (X) = (u i , v i , ρ i );

[0059] (2) According to equation (10), the observation equation of the misaligned image feature point information is expressed as h c (X) = (u i , v i );

[0060] (3) According to equation (11), the observation equation of the misaligned three-dimensional point cloud information is expressed as h l (X) = ρ i ;

[0061] The Jacobian matrix of the observation equation is

[0062]

[0063] The specific process of the filtering algorithm is as Figure 3 shown.

[0064] In this simulation, the Bennu asteroid is selected as the background and simulated in the MATLAB environment. The small celestial body rotates around the Z-axis with a spin angular velocity of 0.233 rad / s. The camera focal length is 125 mm, the image resolution is 1024×1024, and the field of view angle is 20.8°; the camera observation noise satisfies the Gaussian distribution N(0, 2 2 ), the lidar noise satisfies the Gaussian distribution N(0, 0.2 2 ), and the initial pose relationship between the small celestial body and the detector is as Figure 4 shown.

[0065] The initial position of the detector is r = [0::0::1000] T m, the velocity is v = [1::1::2] T m, the attitude quaternion is [0::00::1], the coordinates of feature point 1 are [-96::217::-79] T m, the coordinates of feature point 2 are [114::218::-41] T m, the coordinates of feature point 3 are [-94::138::174] T m, the coordinates of feature point 4 are [92::94::-194] T m. The landing time is 300 frames, and it is assumed that the sampling time interval of the lidar and the camera is 1 s.

[0066] The method for estimating the physical parameters of small celestial bodies in a dim environment of the present invention. Figure a shows the results of the algorithm of the present invention for estimating the spin angular velocity, spin axis direction, position and velocity of small celestial bodies. As can be seen from Figure 4 a and b in, the traditional monocular camera-based estimation methods converge at the 80th and 120th frames respectively. The lidar-based estimation methods converge at the 150th and 200th frames respectively. In the method of the present invention, the rotational angular velocity and the spin axis direction converge at the 20th frame. Compared with the method for estimating the motion state of small celestial bodies based on a single sensor, the method of the present invention can converge quickly. The error of the spin angular velocity of the small celestial body is 0.01 rad / s, and the error of the spin axis direction is 0.05 rad. As can be seen from Figure 4 c and d in, the velocity of the small celestial body converges at the 50th frame, and the error is about 0.2 m / s. The position of the small celestial body converges at the 150th frame, and the error is about 2 m.

[0067] The above specific description further details the purpose, technical solution and beneficial effects of the invention. It should be understood that the above is only a specific embodiment of the present invention and is not used to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for estimating the rotation parameters of small celestial bodies by integrating a camera and lidar, characterized in that It includes the following steps: S1: Define a coordinate system, establish a relative motion model of the small celestial body and the detector, and establish a state equation; ， ； In the formula, and are the position and velocity of the detector relative to the small celestial body in the coordinate system ; is the attitude quaternion of the center of mass of the small celestial body relative to the center of mass of the detector; represents the angular velocity vector of the three axes of the small celestial body coordinate; the spatial coordinates of the image feature points in the center of mass coordinate system of the small celestial body are expressed as ; During the landing process of the detector, a monocular camera acquires navigation images; a lidar acquires depth information of feature points, projects the laser point cloud onto the image plane, and establishes a matching relationship between the image feature points and the depth information of the feature points; assume that the camera measurement of the i-th feature point is: ; Wherein, is the pixel coordinate of the feature point, is the three-dimensional coordinate of the feature point in the camera coordinate system, is the camera focal length; The lidar measurement of the j-th feature point is ; In the formula, is the distance of the j-th feature point relative to the detector, is the position vector of the detector relative to the centroid of the small celestial body; is the position vector of the feature point relative to the centroid of the small celestial body; According to formula (2), the observation equation of the camera is as shown in formula (4) ; In the formula, is the rotation matrix component of the small celestial body coordinate system relative to the camera coordinate system, is the position vector of the small celestial body in the camera coordinates; According to formula (3), the observation equation of the lidar is as shown in formula (5) ; In the formula, , is the rotation matrix of the small celestial body coordinate system relative to the camera coordinate system, is 's three-dimensional coordinates; S2: Define an observation block matrix, fuse various observation information through the block matrix, and realize the estimation of the rotation parameters of the small celestial body.

2. The method for estimating the rotation parameters of small celestial bodies integrating a camera and a lidar according to claim 1, wherein: The step S2 includes: There are three cases for whether the image feature points and the 3D point cloud are aligned: ; In the formula, are the position, velocity, attitude quaternion, and angular velocity of the small celestial body, are the coordinates of the feature points with successful fusion, are the coordinates of the image feature points, are the coordinates of the three-dimensional point cloud; S2-1. According to formula (4) and formula (5), the observation equation of the successfully aligned depth image feature point information is expressed as ; S2-2. According to formula (4), the observation equation of the misaligned image feature point information is expressed as ; S2-3. According to formula (5), the observation equation of the misaligned three-dimensional point cloud information is expressed as ; The Jacobian matrix of the observation equation is ; In the formula, represents the partial derivative of a scalar function with respect to a vector.

Images