A real-time satellite pose estimation method based on perspective N-point geometry algorithm

Abstract

The present invention records a method for real-time estimation of satellite posture based on a perspective N-point geometry algorithm, comprising the following steps: S1, using a depth camera to continuously take images of a target satellite in real time, identifying a high-fidelity image, and extracting and saving the depth information and brightness information of each pixel in the high-fidelity image; S2, using the FAST feature point detection method to extract feature points of two adjacent frames of target satellite high-fidelity images; S3, constructing corresponding BRIEF feature point descriptors for the feature points of two adjacent frames of target satellite high-fidelity images; S4, matching feature points for the feature points extracted from two adjacent frames of target satellite high-fidelity images; S5, estimating the posture of the target satellite based on the perspective N-point geometry method through the matched feature points of the target satellite high-fidelity images. The method for real-time estimation of satellite posture recorded in the present invention performs matching calculations on the feature points of the target satellite image, and can quickly and accurately match the target satellite image in a space environment.

Description

Technical Field

[0001] The present invention relates to the technical field of target object positioning calculation methods, and in particular to a satellite posture real-time estimation method based on a perspective N-point geometry algorithm. Background Art

[0002] At present, countries around the world are investing rapidly in tasks such as space debris removal, failed satellite recovery, satellite fuel filling, and on-orbit maintenance and upgrades. When performing the above tasks, it is necessary to accurately and real-time obtain the current position of the satellite. The position of the satellite is specifically 6D position, including its three-dimensional attitude and three-dimensional position in space.

[0003] The traditional methods for object pose estimation mainly include:

[0004] Drost et al. proposed a method based on point pair feature matching. The method obtains the best pose estimation result by matching the model with the point pair features of the scene. It can achieve good results for noise and partial area occlusion, but the algorithm takes too long.

[0005] Ulrich et al. used the edge contour features of the CAD model to build a hierarchical template pyramid, which can quickly identify three-dimensional objects and estimate their pose in a single image. However, the matching accuracy is easily affected by scale and cannot handle occlusion problems.

[0006] Gavrila et al. proposed to compare the similarity between the input image and the template by calculating the chamfer distance between the template and the input image, and select chamfer matching to obtain the template with the highest similarity to the input image. However, this method has high requirements on image resolution and is easily affected by lighting.

[0007] Hinter stoisser uses the classic template matching method. Even if the target object has insufficient texture, this multimodal feature can obtain more robust estimation results than the method based on single-modal features in complex environments. However, many templates must be used to cover possible viewing angles, which is not suitable for real-time scenarios.

[0008] Kehl proposed a method based on local voting, which predicts the pose through the features of local objects and then filters the final result through a voting algorithm. It has strong robustness to occlusion, but the calculation complexity will increase as multiple local features are calculated.

[0009] Xiang proposed an end-to-end PoseCNN network solution, which can solve the impact of weak texture targets on pose estimation based on RGB images to a certain extent, but is easily affected by occlusion and lighting.

[0010] In summary, it is difficult to accurately obtain the 6D pose of a space satellite in real time in a space environment using traditional target pose estimation methods. Therefore, it is urgent to propose a method that can accurately estimate the satellite pose. Summary of the invention

[0011] In order to solve the above problems existing in the prior art, the present invention provides a satellite posture real-time estimation method based on a perspective N-point geometry algorithm.

[0012] The technical solution of the present invention is:

[0013] A satellite posture real-time estimation method based on a perspective N-point geometry algorithm comprises the following steps:

[0014] S1, uses a depth camera to continuously capture images of the target satellite in real time, identifies the high-fidelity image of the target satellite, and extracts and saves the depth information and brightness information of each pixel in the high-fidelity image;

[0015] S2, using the FAST feature point detection method to extract the feature points of two adjacent frames of high-fidelity target satellite images:

[0016] S3, construct the corresponding BRIEF feature point descriptor for the feature points of two adjacent frames of target satellite high-fidelity images;

[0017] S4, matching the feature points extracted from two adjacent frames of high-fidelity images of the target satellite;

[0018] S5, using the matched target satellite high-fidelity image feature points, estimates the target satellite pose based on the perspective N-point geometry method, specifically including the following steps:

[0019] S501, establishing a coordinate system identity mathematical model of the target satellite according to n frames of target satellite images taken by the depth camera;

[0020] S502, define p n To match the feature points, the feature point of the target satellite in the nth frame image is p n , the coordinate system where the feature point is located is {P n};

[0021] S503, defining the fixed coordinate system of the depth camera in practice as {C1}, defining the first viewing angle when the depth camera takes the first frame of image as the first viewing angle, and its posture relative to the target satellite as the initial posture; setting the depth camera to take the nth frame of image at the nth viewing angle, and setting the posture of the depth camera relative to the target satellite to maintain the initial posture at the nth viewing angle;

[0022] S504, target satellite coordinate system {P nThe relative pose relationship expression of {C1} relative to the depth camera coordinate system is:

[0023] The depth camera coordinate system under the n-th viewing angle {C n The relative position relationship expression of {C1} relative to the fixed coordinate system of the depth camera is:

[0024] Target satellite coordinate system {P n The relative position and posture expression of the depth camera coordinate system {C1}, that is, the real-time attitude expression of the target satellite is:

[0025] Where n≧2, T is the homogeneous transformation matrix.

[0026] When n = 1, the target satellite coordinate system {P n The relative position and posture expression of the depth camera coordinate system {C1}, that is, the real-time attitude expression of the target satellite is: This expression is a known quantity.

[0027] Furthermore, the general expression of the real-time position and posture of the target satellite can be generalized: The calculation formula is:

[0028]

[0029] Where n is the nth frame of the target satellite image where the target satellite is located, and n≧2.

[0030] Furthermore, by minimizing the reprojection error, the relative pose estimation between the depth cameras is solved, and the calculation formula is:

[0031]

[0032] In the formula,

[0033] U i It is a 3*1 matrix, the first two dimensions are the pixel coordinates projected on the 2D plane, and the last dimension is 1;

[0034] P i is a set of feature points in three-dimensional space;

[0035] K is the camera internal parameter;

[0036] s i is the depth value;

[0037] i is the i-th feature point of the current frame image;

[0038] T is the relative position between the fixed actual camera and the virtual perspective camera in the current frame;

[0039] T * It is the optimal relative pose between the fixed actual camera and the virtual perspective camera in the current frame.

[0040] Furthermore, the depth camera is set to continuously capture 25 images of the target satellite within 1 s, and the interval between adjacent images is set to 0.039 s.

[0041] Furthermore, the method for identifying high-fidelity images is: calculating the average brightness of each frame of the captured image, and setting images with an average brightness in the interval of [50, 200] as high-fidelity images.

[0042] Furthermore, the FAST feature point detection method is used to extract feature points of two adjacent frames of target satellite high-fidelity images, including the following steps:

[0043] The FAST feature point detection method is used to check the brightness of the neighborhood circle pixels for each pixel point in the target satellite high-fidelity image, specifically checking the brightness of the 1st, 5th, 9th, and 13th pixel points in the neighborhood circle pixel points;

[0044] If the brightness of three of the four pixels, 1, 5, 9, and 13, in the neighborhood circle of the target pixel are simultaneously greater than the brightness of the target pixel or simultaneously less than the brightness of the target pixel, the target pixel is set as a feature point of the target satellite high-fidelity image.

[0045] Furthermore, for the feature points of two adjacent frames of target satellite high-fidelity images, the corresponding BRIEF feature point descriptor is constructed, which specifically includes the following steps: defining a key point in the image; selecting a neighborhood of a fixed size around the key point, and randomly selecting N point pairs in the neighborhood; comparing the pixel values ​​of the two points in each point pair, and generating an N-dimensional binary vector as the feature point descriptor according to the comparison result.

[0046] Furthermore, key points refer to the corner point detection of an image, that is, the area in the image where the local pixel grayscale changes significantly.

[0047] Furthermore, a neighborhood of a fixed size refers to an area with a fixed radius of 8 pixels, that is, an image with a pixel size of 16*16.

[0048] Furthermore, feature point matching is performed on the feature points extracted from two adjacent frames of target satellite high-fidelity images. The specific method is: the Hamming distance is calculated pairwise for the feature point descriptors of the two adjacent frames of target satellite high-fidelity images, and the feature points corresponding to the two feature point descriptors with the smallest Hamming distance are the matched feature points.

[0049] The beneficial effects of the present invention are:

[0050] The satellite posture real-time estimation method recorded in the present invention performs matching calculations on the feature points of the target satellite image, and can quickly and accurately match the target satellite image in a space environment. Based on the perspective N-point geometry algorithm, the estimation efficiency and accuracy of the target satellite are effectively improved. Therefore, the method recorded in the present invention can effectively improve the efficiency of operations such as capturing and repairing the target satellite. At the same time, the method recorded in the present invention is also applicable to the real-time estimation of the 6D posture of target objects in other aerospace applications, and has good practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 : A schematic diagram of neighborhood circle pixels in an exemplary embodiment of the present invention;

[0052] Figure 2 : A schematic diagram of a mathematical model between the target satellite coordinate systems of the two frames of image modeling in an exemplary embodiment of the present invention;

[0053] Figure 3 : Schematic diagram of reprojection error in an exemplary embodiment of the present invention. DETAILED DESCRIPTION

[0054] The present invention is described in detail below by way of embodiments in conjunction with the accompanying drawings.

[0055] The present invention records a satellite posture real-time estimation method based on a perspective N-point geometry algorithm, which specifically includes the following steps.

[0056] S1 uses a depth camera to continuously capture images of the target satellite in real time and identify high-fidelity images of the target satellite.

[0057] a. Use a depth camera to continuously shoot the target satellite in real time. It is preferably set to shoot 25 images continuously within 1 second, and the shooting interval between adjacent images is set to 0.039 seconds.

[0058] b. The method for identifying high-fidelity images from the captured target satellite real-time images is: calculate the average brightness of each frame of the captured image, and set the image with an average brightness in the range of [50,200] as a high-fidelity image.

[0059] The method of averaging brightness is as follows: first, a histogram is made for the brightness values ​​of each frame of the image, and then a cumulative average is made for each brightness value.

[0060] S2, extracts and saves the depth information and brightness information of each pixel in the high-fidelity image of the target satellite.

[0061] In this step, the depth information of the pixel can be directly acquired by the depth camera, that is, it can be obtained by directly calling the image depth information function.

[0062] S3, extracting feature points of two adjacent frames of high-fidelity images of the target satellite. The specific steps are as follows:

[0063] S301, using the FAST feature point detection method, checks the brightness of the neighborhood circle pixels for each pixel of the target satellite image, specifically checks the brightness of the 1st, 5th, 9th, and 13th pixel points in the neighborhood circle pixels, Figure 1 A schematic diagram of checking the brightness of pixels in a neighborhood circle is given.

[0064] S302, a method for determining a pixel feature point of a target satellite image is as follows: if the brightness of the first, fifth, ninth, and thirteenth pixels in the neighborhood circle of the target pixel is greater than the brightness of the target pixel or less than the brightness of the target pixel at the same time, the target pixel is set as a feature point of the target satellite image.

[0065] S4, construct the corresponding BRIEF feature point descriptor for the feature points of two adjacent frames of target satellite high-fidelity images. The specific steps are as follows:

[0066] a. Define a key point in the image;

[0067] b. Select a neighborhood of fixed size around the key point and randomly select N point pairs in the neighborhood;

[0068] c. Compare the pixel values ​​of the two points in each point pair, and generate an N-dimensional binary vector as a feature point descriptor based on the comparison result.

[0069] In this step,

[0070] 1. Key points refer to the corner point detection of the image, that is, the area where the local pixel grayscale changes significantly in the image;

[0071] 2. A fixed-size neighborhood refers to an area with a fixed radius of 8 for the pixel, that is, an image with a pixel size of 16*16;

[0072] 3. Randomly select N point pairs using a method based on random seed number;

[0073] 4. Comparing the pixel values ​​of the two points in each point pair is to compare the brightness of the two points in each point pair;

[0074] 5. Select 2 pixels using the random seed number method, compare the brightness of the two pixels, store them as 0 and 1, and generate an N-dimensional binary vector.

[0075] S5, performing feature point matching on the feature points extracted from two adjacent frames of target satellite high-fidelity images. The specific method is: calculating the Hamming distance between the feature point descriptors of the two adjacent frames of target satellite high-fidelity images, and the feature points corresponding to the two feature point descriptors with the smallest Hamming distance are the matched feature points.

[0076] S6, using the matched target satellite high-fidelity image feature points, estimates the target satellite pose based on the perspective N-point geometry method, specifically including the following steps:

[0077] For ease of description, this step takes the two frames of high-fidelity images of the target satellite taken by the depth camera as an example to perform the method and calculation of estimating the target satellite pose, and defines the two frames of target satellite images as the first frame image and the second frame image.

[0078] S601, as attached Figure 2 As shown, based on the two frames of target satellite images taken by the depth camera, a coordinate system identity mathematical model of the target satellite is established.

[0079] S602, define p1 and p2 as matched feature points; the feature point of the target satellite in the first frame image is p1, and the coordinate system where the feature point is located is {P1}; the feature point of the target satellite in the second frame image is p2, and the coordinate system where the feature point is located is {P2}.

[0080] S603, define the fixed coordinate system of the depth camera in reality as {C1}, define the first viewing angle when the first frame of image is taken as the first viewing angle, and its posture relative to the target satellite is the initial posture. Therefore, it can be seen that the first viewing angle of the depth camera is the real viewing angle.

[0081] During the time between the two frames of images taken by the depth camera, the target satellite is transformed from the coordinate system {P1} in the first frame to the coordinate system {P2} in the second frame after rotation and translation;

[0082] Since the fixed coordinate system {C1} of the depth camera has not changed, its posture relative to the target satellite when shooting the second frame is no longer the initial posture. Now the depth camera is set to shoot the second frame at a second viewing angle, and the posture of the depth camera relative to the target satellite is set to maintain the initial posture under the second viewing angle. It can be seen that the second viewing angle of the depth camera is a virtual viewing angle.

[0083] S604, based on the above steps S601-S603, there are:

[0084] The relative position relationship expression of the target satellite coordinate system {P1} relative to the camera coordinate system {C1} is:

[0085] The relative position relationship expression of the target satellite coordinate system {P2} relative to the camera coordinate system {C1} is:

[0086] The relative position relationship expression of the target satellite coordinate system {P2} relative to the camera coordinate system {C2} is:

[0087] Where T is the homogeneous transformation matrix, which is used to describe the relative position and direction relationship between different coordinate systems.

[0088] In the above posture relationship expression, the expression used to describe the real-time posture estimation of the target satellite is: That is, it is used to describe the real-time position and posture of the target satellite after it translates and rotates to the second frame image.

[0089] S605, calculating and solving the relative posture relationship expression in step S604, specifically:

[0090] According to step S2, the depth information of the image pixels can be directly obtained by the depth camera. Therefore, the coordinate system {P1} of the target satellite in the first frame image is a known quantity, and the relative position of the coordinate system {P1} relative to the coordinate system {C1} is The same is a known quantity;

[0091] Since the coordinate system {P2} of the target satellite in the second frame image is taken by the depth camera in a non-initial posture, the coordinate system {P2} is an unknown quantity; the relative position of the coordinate system {P2} with respect to the coordinate system {C1} is is the target pose to be solved.

[0092] Since the depth camera is set to shoot the second frame image of the target satellite at a virtual second viewing angle, and the second viewing angle maintains the same initial posture relative to the second frame image of the target satellite,

[0093] because and Having the same posture, the posture transformation matrix of {P2} relative to {C2} is:

[0094]

[0095] By formula Solve the position of the satellite coordinate system {P2} relative to the camera coordinate system {C1} in the second frame image because Then we have:

[0096]

[0097] In the formula, Represents the relative pose of the camera coordinate system {C2} relative to the fixed camera coordinate system {C1} under the second perspective. The relative pose between depth cameras is calculated by minimizing the reprojection error. Here, when there are multiple frames of images, that is, when the target satellite rotates and translates to the nth position, the relative pose estimation solution between the depth cameras is:

[0098] Minimizing the reprojection error is briefly described as follows: In the depth camera model, a pair of matching points P1 and P2 are known. The matching points are formed by projecting the same spatial point P onto two different camera planes. Assuming that the relative position between the two depth cameras is T, the relative position T is obtained by constructing a least squares problem to calculate the minimized reprojection error. Then point P1 can be projected to P2' after relative transformation. Figure 3 As shown in the figure, due to the noise in the satellite estimation, P2 and P2' in the ideal state will not coincide, and there will be a reprojection error e. By continuously adjusting the relative position T between the cameras, the reprojection error e can be minimized.

[0099] Based on the above description, the problem is constructed as a least squares problem. The least squares problem is a numerical approximation problem. The discrete square approximation technique is used to find the fitting curve. It is usually used to find the best fitting model, especially in the presence of noise or error data, by minimizing its error T. * To find the best relative pose between depth cameras, the calculation formula is:

[0100]

[0101] In the formula,

[0102] U i It is a 3*1 matrix, the first two dimensions are the pixel coordinates projected on the 2D plane, and the last dimension is 1;

[0103] P i is a set of feature points in three-dimensional space;

[0104] K is the camera internal parameter;

[0105] s i is the depth value;

[0106] i is the i-th feature point of the current frame image. Calculating the relative pose between the two cameras requires information on many feature points of the previous and next two frames of images.

[0107] T is the relative position between the fixed actual camera and the virtual perspective camera in the current frame;

[0108] T *It is the optimal relative pose between the fixed actual camera and the virtual perspective camera in the current frame.

[0109] S7, since the target satellite is constantly rotating and translating during operation, the depth camera continuously shoots in the actual camera coordinate system {C1}, and its virtual perspective is sequentially expressed as the 2nd to nth virtual perspectives, so that the current frame target satellite image taken at each virtual perspective maintains the same posture as the previous key frame target satellite image. Therefore, the subsequent real-time attitude estimation of the target satellite can be expressed by the following general formula:

[0110]

[0111] Since in the first frame image, the data of the target satellite's spatial three-dimensional posture is set to the same posture as the depth camera, and the posture coordinates of the depth camera itself are represented as fixed values, therefore, n is the nth frame target satellite image where the target satellite is located, that is, it represents the point representing the position and posture of the target satellite in the nth frame image, and n≧2.

[0112] Taking n as 2-4 as an example, the solution of the calculation formula is explained:

[0113] When the satellite is in the first frame of the image, its real-time pose expression is:

[0114] When n=2, when the satellite is in the second frame, its real-time posture expression is:

[0115]

[0116] When n=3, when the satellite is in the third frame of image, its real-time posture expression is:

[0117]

[0118] When n=4, when the satellite is in the fourth frame, its real-time posture expression is:

[0119]

[0120] It can be seen that as the target satellite continuously rotates and translates to change its position, its sequence appears in the nth frame of satellite image, and its position and posture expressions are no longer exhaustively listed here.

[0121] The calculation formula of has been described in detail in step S6, where n represents the nth key frame image, and will not be repeated here.

[0122] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments, and the above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, and these changes and improvements all fall within the scope of the present invention to be protected.

Claims

1. A method for real-time satellite pose estimation based on a perspective N-point geometry algorithm, characterized in that: The following steps are involved: S1, uses a depth camera to continuously capture images of the target satellite in real time, identifies the high-fidelity image of the target satellite, and extracts and saves the depth information and brightness information of each pixel in the high-fidelity image; S2, using the FAST feature point detection method to extract feature points of two adjacent frames of high-fidelity images of the target satellite; S3, construct the corresponding BRIEF feature point descriptor for the feature points of two adjacent frames of target satellite high-fidelity images; S4, matching the feature points extracted from two adjacent frames of high-fidelity images of the target satellite; S5, using the matched target satellite high-fidelity image feature points, estimates the target satellite pose based on the perspective N-point geometry method, specifically including the following steps: S501, establishing a coordinate system identity mathematical model of the target satellite according to n frames of target satellite images taken by the depth camera; S502, define p n To match the feature points, the feature point of the target satellite in the nth frame image is p n , the coordinate system where the feature point is located is {P n }; S503, defining the fixed coordinate system of the depth camera in practice as {C1}, defining the first viewing angle when the depth camera takes the first frame of image as the first viewing angle, and its posture relative to the target satellite as the initial posture; setting the depth camera to take the nth frame of image at the nth viewing angle, and setting the posture of the depth camera relative to the target satellite to maintain the initial posture at the nth viewing angle; S504, target satellite coordinate system {P n The relative pose relationship expression of {C1} relative to the depth camera coordinate system is: The depth camera coordinate system under the n-th viewing angle {C n The relative position relationship expression of {C1} relative to the fixed coordinate system of the depth camera is: Target satellite coordinate system {P n The relative position and posture expression of the depth camera coordinate system {C1}, that is, the real-time attitude expression of the target satellite is: Where n≧2, T is the homogeneous transformation matrix; Real-time attitude expression of target satellite The calculation formula is: Where n is the nth frame of the target satellite image where the target satellite is located, and n≧2; By minimizing the reprojection error, the relative pose estimation between depth cameras is solved. The calculation formula is: In the formula, U i It is a 3*1 matrix, the first two dimensions are the pixel coordinates projected on the 2D plane, and the last dimension is 1; P i is a set of feature points in three-dimensional space; K is the camera internal parameter; s i is the depth value; i is the i-th feature point of the current frame image; T is the relative position between the fixed actual camera and the virtual perspective camera in the current frame; T * It is the optimal relative pose between the fixed actual camera and the virtual perspective camera in the current frame.

2. The method for real-time satellite pose estimation based on a perspective N-point geometry algorithm according to claim 1, characterized in that: The depth camera is set to take 25 images of the target satellite continuously within 1 second, and the interval between adjacent images is set to 0.039 seconds.

3. The method for real-time satellite pose estimation based on perspective N-point geometry algorithm according to claim 1, characterized in that: The method for identifying high-fidelity images is: calculating the average brightness of each frame of the captured image, and setting the images with average brightness in the interval [50,200] as high-fidelity images.

4. The method for real-time satellite pose estimation based on perspective N-point geometry algorithm according to claim 1, characterized in that: The FAST feature point detection method is used to extract the feature points of two adjacent frames of target satellite high-fidelity images, including the following steps: The FAST feature point detection method is used to check the brightness of the neighborhood circle pixels for each pixel point in the target satellite high-fidelity image, specifically checking the brightness of the 1st, 5th, 9th, and 13th pixel points in the neighborhood circle pixel points; If the brightness of three of the four pixels, 1, 5, 9, and 13, in the neighborhood circle of the target pixel are simultaneously greater than the brightness of the target pixel or simultaneously less than the brightness of the target pixel, the target pixel is set as a feature point of the target satellite high-fidelity image.

5. The method for real-time satellite pose estimation based on perspective N-point geometry algorithm according to claim 1, characterized in that: For the feature points of two adjacent frames of target satellite high-fidelity images, the corresponding BRIEF feature point descriptor is constructed, which specifically includes the following steps: defining a key point in the image; selecting a neighborhood of a fixed size around the key point, and randomly selecting N point pairs in the neighborhood; comparing the pixel values ​​of the two points in each point pair, and generating an N-dimensional binary vector as the feature point descriptor based on the comparison result.

6. The method for real-time satellite pose estimation based on perspective N-point geometry algorithm according to claim 5, characterized in that: Key points refer to the corner detection of an image, that is, the area where the local pixel grayscale changes significantly in the image.

7. The method for real-time satellite pose estimation based on perspective N-point geometry algorithm according to claim 5, characterized in that: A fixed-size neighborhood refers to an area with a fixed radius of 8 pixels, that is, an image with a pixel size of 16*16.

8. The method for real-time satellite pose estimation based on perspective N-point geometry algorithm according to claim 1, characterized in that: Feature point matching is performed on the feature points extracted from two adjacent frames of target satellite high-fidelity images. The specific method is: the Hamming distance is calculated pairwise for the feature point descriptors of the two adjacent frames of target satellite high-fidelity images. The feature points corresponding to the two feature point descriptors with the smallest Hamming distance are the matched feature points.

Images