Spatial cooperative target pose resolving method and system based on monocular vision

Abstract

The invention provides a relative pose resolving method and system suitable for a space cooperative target, and the method comprises the steps: building a matrix based on the internal reference of a camera, and solving an inverse matrix; obtaining ideal image point positions of at least four target points on a phase plane based on the inverse matrix; constructing a collinear projection equation based on the ideal image point position; obtaining a solution of the collinear projection equation in combination with an external parameter of a target point; calculating a rotation axis and a rotation angle by using the solution of the collinear projection equation; and converting the rotation axis and the rotation angle into a rotation matrix to obtain the relative pose of the cooperative target. According to the method, the resolving precision can be effectively improved, the error tolerance is higher, the robustness of a product is remarkably improved, and the method is more suitable for engineering application. And the method has an important application value in the aspect of space cooperative target pose calculation.

Description

Technical Field

[0001] The present invention relates to the field of space station rendezvous and docking, rendezvous measurement between spacecraft, spacecraft launch and recovery measurement, and relative measurement of other cooperative targets, and in particular, to a method, system, terminal, and medium for solving the position and posture of space cooperative targets based on monocular vision. Background Art

[0002] The method for solving the pose of spatial cooperative targets based on monocular vision is a method that obtains the image features of cooperative targets through a single camera and solves the relative pose relationship between the target and the camera based on the image features. For cooperative point targets, a point correspondence-based method is usually used. It was first proposed by Fischler to solve the pose between the target and the camera by controlling the constraints of the position relationship of the target points, also known as the PnP (perspective n points) problem. However, when the actual monocular measurement system uses the PnP method to directly solve the pose, it is usually inevitable to introduce many errors, such as feature point centroid extraction error, external parameter calibration error, coplanar point flatness error, camera internal parameter calibration error, etc., which will lead to large errors in the solution results and even cause mismatching of points, which will make it difficult for subsequent iterative methods to work normally, thereby directly affecting the accuracy and robustness of the pose information. Summary of the invention

[0003] In view of the defects in the prior art, the purpose of the present invention is to provide a method, system, terminal and medium for solving the pose of spatial cooperative targets based on monocular vision.

[0004] According to one aspect of the present invention, a relative posture solution method applicable to space cooperation targets is provided, comprising:

[0005] Based on the camera intrinsic parameters, construct the matrix and solve the inverse matrix;

[0006] Based on the inverse matrix, obtaining the ideal image point positions of at least four target points on the phase plane;

[0007] Based on the ideal image point positions, constructing collinear projection equations;

[0008] Combining the external parameters of the target point, obtaining a solution to the collinear projection equation;

[0009] Calculating the rotation axis and the rotation angle using the solution of the collinear projection equation;

[0010] The rotation axis and the rotation angle are converted into a rotation matrix to obtain the relative position and posture of the cooperation target.

[0011] Preferably, constructing a matrix and solving an inverse matrix based on the camera intrinsic parameters includes:

[0012] According to the lens focal length f in the camera intrinsic parameters, establish the matrix K:

[0013] Based on the matrix K, the inverse matrix K-1 is obtained: K -1 =inverse(K), inverse is the matrix inverse function.

[0014] Preferably, obtaining the ideal image point positions of the four target points on the phase plane based on the inverse matrix includes:

[0015] Solve the phase plane projection points of the four target points:

[0016] Using the projection coordinates of the target point on the phase plane xi 、p yi , get the ideal image point position, that is, the generalized coordinate representation of the target vector cp xi 、cp yi .

[0017] Preferably, the collinear projection equation is constructed based on the ideal image point position:

[0018]

[0019] The external parameter {P x1 ,P y1 ,P x2 ,P y2 ,P x3 ,P y3 ,P x4 ,P y4}, obtaining the solution of the collinear projection equation, including:

[0020] Solving the collinear projection equation, the solution of the equation is obtained as: {sol1, sol2, sol3, sol4, sol5, sol6, sol7, sol8};

[0021] Normalizing the solution of the equation to obtain a normalized solution {r0, r1, r2, r3, r4, r5, r6, r7} and a translation {Tx, Ty, Tz};

[0022]

[0023] Preferably, the calculating the rotation axis and the rotation angle using the solution of the collinear projection equation comprises:

[0024] First, solve the three-variable linear equation ax 3 +bx 2 +cx+d=0, using the real number solution of solar1, we get the rotation axis evector, where

[0025]

[0026]

[0027] Normalize is the normalization function of the vector;

[0028] Next, calculate the rotation angle, including:

[0029] Calculate eanglecos, The trace function is used to find the trace of the matrix.

[0030]

[0031] Calculate eangle sin timesn,

[0032] eanglesintimesn=R origin -eanglecos*identitymatrix-(1-eanglecos)kroneckerproduct[evector,evector]

[0033] The trace function is used to find the trace of the matrix, identitymatrix is ​​a 3*3 unit matrix, and kroneckerproduct is a function for finding the outer product of vectors;

[0034] Calculate eanglesin using eanglesintimesn and the matrix element of evector.

[0035]

[0036] Get the rotation angle:

[0037]

[0038] Preferably, converting the rotation axis and the rotation angle into a rotation matrix to obtain the relative position and posture of the cooperation target includes:

[0039] Convert the rotation axis and the rotation angle into a rotation matrix R:

[0040] R=cos(eangle)identitymatrix+(1-cos(eangle))kroneckerproduct(evector,evector)+sin(eangle)antisymetrymatrix(evector)

[0042] Where antisymetrymatrix is ​​an antisymmetric matrix function;

[0043] Calculate the Euler angles {α, β, γ} using the matrix elements of the rotation matrix R

[0044]

[0045] The pose from the target plate to the camera is obtained as {α, β, γ, T x , T y , T z}.

[0046] According to a second aspect of the present invention, a relative posture solving system applicable to space cooperation targets is provided, comprising:

[0047] Inverse matrix module, which constructs the matrix and solves the inverse matrix based on the camera intrinsic parameters;

[0048] A position module, which obtains the ideal image point positions of four target points on the phase plane based on the inverse matrix;

[0049] An equation module, which constructs a collinear projection equation based on the ideal image point position;

[0050] A solution module, which obtains a solution to the collinear projection equation by combining the external parameters of the target point;

[0051] A Lie algebra module that uses the solutions of the collinear projection equations to calculate the rotation axis and the rotation angle;

[0052] A posture module converts the rotation axis and the rotation angle into a rotation matrix to obtain the relative posture of the cooperative target.

[0053] According to a third aspect of the present invention, there is provided an application of the relative position and posture of a space cooperation target. Based on the relative position and posture solution method applicable to the space cooperation target or based on the relative position and posture solution system applicable to the space cooperation target, the relative position and posture of the space cooperation target is obtained. Based on the relative position and posture, space station rendezvous and docking, rendezvous between spacecraft, or launch and recovery of spacecraft are carried out.

[0054] According to a fourth aspect of the present invention, there is provided a terminal, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor can be used to execute the method described, or to run the system described, when executing the program.

[0055] According to a fifth aspect of the present invention, there is provided a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, can be used to execute the method described, or to run the system described.

[0056] Compared with the prior art, the embodiments of the present invention have at least one of the following beneficial effects:

[0057] The method and system for solving the spatial cooperative target posture based on monocular vision in the embodiment of the present invention effectively improves the solving accuracy, has a higher error tolerance, significantly improves the robustness of the product, and is more suitable for engineering applications. It has important application value in solving the spatial cooperative target posture. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0059] Figure 1 It is a flow chart of a method for solving spatial cooperative target pose based on monocular vision in one embodiment of the present invention;

[0060] Figure 2 A schematic diagram of the configuration of a cooperative target in a preferred embodiment of the present invention;

[0061] Figure 3 Graph showing the relationship between the target plate and the phase plane in a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0062] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several variations and improvements may be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0063] In order to overcome the shortcomings of the prior art, solve the uncertainty of the traditional PnP method in engineering applications, and improve the robustness of the product, the present invention proposes a relative pose solution method suitable for space cooperation targets, as detailed in the following embodiments.

[0064] In one embodiment of the present invention, a relative posture solution method suitable for space cooperation targets is provided, such as Figure 1 As shown, the main process is:

[0065] S100, constructing a matrix and solving the inverse matrix based on the camera intrinsic parameters;

[0066] S200, based on the inverse matrix obtained in S100, obtaining the ideal image point positions of at least four target points on the phase plane;

[0067] S300, constructing a collinear projection equation based on the ideal image point position obtained in S200;

[0068] S400, combining the external parameters of the target point to obtain the solution of the collinear projection equation of S300;

[0069] S500, calculating the rotation axis and the rotation angle using the solution of the collinear projection equation obtained in S400;

[0070] S600, converting the rotation axis and rotation angle obtained in S500 into a rotation matrix to obtain the relative position and posture of the cooperative target.

[0071] The posture information obtained by the method of the above embodiment can effectively improve the solution accuracy through experimental testing, and has a higher error tolerance, which significantly improves the robustness of the product and is more suitable for engineering applications.

[0072] In a preferred embodiment of the present invention, a preferred configuration of the cooperative target is provided, such as Figure 2 As shown, this is the configuration of the target plate. The periphery (1)(2)(3)(4) are four coplanar points. Any three points are not colinear. When using, their posture relationship needs to be calibrated in advance.

[0073] In other embodiments, more than 4 target points may be used. If more than 4, four points with the farthest sum of distances from each other are selected, and the remaining target points can further improve the accuracy in subsequent calculations.

[0074] In a preferred embodiment of the present invention, S100 is implemented to construct a camera intrinsic parameter matrix using the focal length of the camera, and to invert it, to construct a K matrix according to formula (1), and to obtain its inverse matrix K -1 .

[0075]

[0076] Where: f is the focal length of the lens, and inverse is the matrix inversion function.

[0077] In a preferred embodiment of the present invention, S200 is implemented to find the ideal image point positions of the four target points on the phase plane, and the phase plane projection point solution is performed according to formula (2), such as Figure 3 shown.

[0078]

[0079] Using the projection coordinates of the target point on the phase plane xi 、p yi , get the ideal image point position, that is, the generalized coordinate representation of the target vector cp xi 、cp yi .

[0080] In a preferred embodiment of the present invention, S300 is implemented to construct a collinear projection equation based on the ideal image point position obtained in S200 as follows:

[0081]

[0082] In a preferred embodiment of the present invention, S400 is implemented, the collinear projection equation is solved in combination with the target point external parameter, and the solution {sol1, sol2, sol3, sol4, sol5, sol6, sol7, sol8} of equation (3) is obtained, and the settlement result is normalized using equation (4) to obtain the normalized solution {r0, r1, r2, r3, r4, r5, r6, r7} and the translation {Tx, Ty, Tz}. Specifically,

[0083]

[0084]

[0085] Where: {P x1 , P y1 , P x2 , P y2 , P x3 , P y3 , P x4 , P y4} are the coordinates of the four target points in the target board coordinate system, and T = {Tx, Ty, Tz} is the translation of the target board relative to the camera.

[0086] In a preferred embodiment of the present invention, S500 is implemented to calculate the rotation axis and the rotation angle using the solution of the collinear projection equation, specifically:

[0087] First, according to equation (5), solve the three-variable linear equation (6) to obtain the rotation axis evector.

[0088]

[0089] ax 3 +bx 2 +cx+d=0 (6)

[0090] Let the real root of equation (6) be sola1, and the rotation axis evector is obtained as equation (7), and Normalize is the normalization function of the vector.

[0091]

[0092] Next, the rotation angle eangle is calculated according to equations (8) to (10).

[0093] Eanglecos is calculated according to formula (8), where the trace function is used to find the trace of the matrix.

[0094]

[0095] Eanglesintimesn is calculated according to formula (9), where the trace function is used to find the trace of the matrix.

[0096] eanglesintimesn=R origin -eanglecos*identitymatrix-(1-eanglecos)kroneckerproduct[evector,evector] (9)

[0098] Where identitymatrix is ​​a 3*3 unit matrix, and kroneckerproduct is a function for finding the outer product of vectors.

[0099] Calculate eanglesin according to formula (10).

[0100]

[0101] The calculated rotation angle is as follows (11):

[0102]

[0103] In a preferred embodiment of the present invention, S600 is implemented to convert the rotation axis and the rotation angle into a rotation matrix, solve the posture, and calculate the Euler angles {α, β, γ} according to formula (12).

[0104] R=cos(eangle)identitymatrix+(1-cos(eangle))kroneckerproduct(evector,evector)

[0105] +sin(eangle)antisymetrymatrix(evector) (12)

[0106] Where antisymetrymatrix is ​​an antisymmetric matrix function.

[0107] The Euler angles {α, β, γ} are expressed as follows (13).

[0108]

[0109] The final calculated target plate to camera pose is {α, β, γ, T x , T y , T z}.

[0110] The above embodiment uses Lie algebra for further processing based on the traditional PNP method, calculates the closest rotation axis and rotation angle representation, and converts it back to the standard rotation matrix to obtain the posture information. Lie algebra is a non-associative algebra. Since it deletes the multiplication associative law in the definition of associative algebra and meets a series of requirements for processing rotation groups, it can provide great convenience in solving the posture solution process, while reducing the amount of calculation and improving the accuracy.

[0111] Based on the same inventive concept, in other embodiments of the present invention, a relative posture solution system suitable for space cooperative targets is provided, including an inverse matrix module, a position module, an equation module, a solution module, a Lie algebra module and a posture module. The inverse matrix module constructs a matrix and solves the inverse matrix based on the camera intrinsic parameters; the position module obtains the ideal image point positions of at least 4 target points on the phase plane based on the inverse matrix; the equation module constructs the collinear projection equation based on the ideal image point positions; the solution module obtains the solution of the collinear projection equation in combination with the external parameters of the target points; the Lie algebra module calculates the rotation axis and rotation angle using the solution of the collinear projection equation; the posture module converts the rotation axis and the rotation angle into a rotation matrix to obtain the relative posture of the cooperative target.

[0112] The specific implementation techniques of each module / unit in the above-mentioned example of the present invention may refer to the corresponding steps of the relative posture solution method applicable to space cooperation targets in the above-mentioned embodiment, which will not be repeated here.

[0113] Based on the same inventive concept, in other embodiments of the present invention, an application of the relative posture of a space cooperation target is provided, and based on the above-mentioned relative posture solution method applicable to space cooperation targets or based on the above-mentioned relative posture solution system applicable to space cooperation targets, the relative posture of the space cooperation target is obtained, and based on the relative posture, space station rendezvous and docking, rendezvous between spacecraft, or launch and recovery of spacecraft are carried out.

[0114] Based on the same inventive concept, in other embodiments of the present invention, a terminal is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor can be used to execute the above-mentioned method or to run the above-mentioned system when executing the program.

[0115] Optionally, the memory is used to store programs; the memory may include volatile memory (English: volatile memory), such as random-access memory (English: random-access memory, abbreviated: RAM), such as static random-access memory (English: static random-access memory, abbreviated: SRAM), double data rate synchronous dynamic random access memory (English: Double Data Rate Synchronous Dynamic Random Access Memory, abbreviated: DDR SDRAM), etc.; the memory may also include non-volatile memory (English: non-volatile memory), such as flash memory (English: flash memory). The memory is used to store computer programs (such as applications, functional modules, etc. that implement the above method), computer instructions, etc., and the above-mentioned computer programs, computer instructions, etc. can be partitioned and stored in one or more memories.

[0116] The processor is used to execute the computer program stored in the memory to implement the various steps in the method involved in the above embodiment. For details, please refer to the relevant description in the above method embodiment.

[0117] The processor and the memory may be independent structures or integrated structures. When the processor and the memory are independent structures, the memory and the processor may be coupled and connected via a bus.

[0118] Based on the same inventive concept, in other embodiments of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the program is executed by a processor, it can be used to execute the above method, or run the above system.

[0119] Among them, computer-readable media include computer storage media and communication media, wherein the communication media include any media that facilitates the transmission of computer programs from one place to another. The storage medium can be any available medium that can be accessed by a general or special-purpose computer. An exemplary storage medium is coupled to the processor so that the processor can read information from the storage medium and write information to the storage medium. Of course, the storage medium can also be an integral part of the processor. The processor and the storage medium can be located in an ASIC. In addition, the ASIC can be located in a user device. Of course, the processor and the storage medium can also be present in a communication device as discrete components.

[0120] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0121] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0122] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0123] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0124] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various modifications or variations within the scope of the claims, which does not affect the essence of the present invention. The above preferred features can be used in any combination without conflicting with each other.

Claims

1. A relative pose solution method suitable for space cooperation targets, characterized in that: include: Based on the camera intrinsic parameters, construct the matrix and solve the inverse matrix; Based on the inverse matrix, obtaining the ideal image point positions of at least four target points on the phase plane; Based on the ideal image point positions, constructing collinear projection equations; Combining the external parameters of the target point, obtaining a solution to the collinear projection equation; Calculating the rotation axis and the rotation angle using the solution of the collinear projection equation; The rotation axis and the rotation angle are converted into a rotation matrix to obtain the relative position and posture of the cooperation target.

2. A relative posture solution method suitable for space cooperation targets according to claim 1, characterized in that: The process of constructing a matrix and solving an inverse matrix based on the camera intrinsic parameters includes: According to the lens focal length f in the camera intrinsic parameters, establish the matrix K: Based on the matrix K, the inverse matrix K is obtained -1 :K -1 =inverse, inverse is the matrix inverse function.

3. A relative posture solution method suitable for space cooperation targets according to claim 2, characterized in that: The method of obtaining the ideal image point positions of the four target points on the phase plane based on the inverse matrix includes: Solve the phase plane projection points of the four target points: Using the projection coordinates of the target point on the phase plane xi 、p yi , get the ideal image point position, that is, the generalized coordinate representation of the target vector cp xi 、cp yi .

4. A relative posture solution method applicable to space cooperation targets according to claim 3, characterized in that: Based on the ideal image point position, a collinear projection equation is constructed: The external parameter {P x1 ,P y1 ,P x2 ,P y2 ,P x3 ,P y3 ,P x4 ,P y4 }, obtaining the solution of the collinear projection equation, including: Solving the collinear projection equation, the solution of the equation is obtained as: {sol1, sol2, sol3, sol4, sol5, sol6, sol7, sol8}; Normalizing the solution of the equation to obtain a normalized solution {r0, r1, r2, r3, r4, r5, r6, r7} and a translation {Tx, Ty, Tz}; 5. A relative posture solution method applicable to space cooperation targets according to claim 4, characterized in that: The method of calculating the rotation axis and the rotation angle by using the solution of the collinear projection equation comprises: First, solve the three-variable linear equation ax 3 +bx 2 +cx+d=0, using the real number solution of solar1, we get the rotation axis evector, where Normalize is the normalization function of the vector; Next, calculate the rotation angle, including: Calculate eanglecos, The trace function is used to find the trace of the matrix. Calculate eangle sin timesn, eangleesintimesn=R origin -eanglecos*identitymatrix-(1-eanglecos)kroneckerproduct[evector,evector] The trace function is used to find the trace of the matrix, identitymatrix is ​​a 3*3 unit matrix, and kroneckerproduct is a function for finding the outer product of vectors; Calculate eanglesin using eanglesintimesn and the matrix element of evector. Get the rotation angle:

6. A relative posture solution method applicable to space cooperation targets according to claim 5, characterized in that: The step of converting the rotation axis and the rotation angle into a rotation matrix to obtain the relative position and posture of the cooperative target includes: Convert the rotation axis and the rotation angle into a rotation matrix R R=cos(eangle)identitymatrix+(1-cos(eangle))kroneckerproduct(evector,eUector)+sin(eangle)antisymetrymatrix(evector) Where antisymetrymatrix is ​​an antisymmetric matrix function; Calculate the Euler angles {α, β, γ} using the matrix elements of the rotation matrix R The pose from the target plate to the camera is obtained as {α, β, γ, T x , T y , T z }.

7. A relative posture solving system suitable for space cooperation targets, characterized in that: include: The inverse matrix module constructs the matrix and solves the inverse matrix based on the camera intrinsic parameters; A position module, which obtains the ideal image point positions of at least four target points on the phase plane based on the inverse matrix; An equation module, which constructs a collinear projection equation based on the ideal image point position; A solution module, which obtains a solution to the collinear projection equation by combining the external parameters of the target point; A Lie algebra module that uses the solutions of the collinear projection equations to calculate the rotation axis and the rotation angle; A posture module converts the rotation axis and the rotation angle into a rotation matrix to obtain the relative posture of the cooperative target.

8. An application method for relative position and posture of space cooperation targets, characterized in that: include: Based on the relative posture solving method applicable to space cooperation targets described in claims 1 to 6 or based on the relative posture solving system applicable to space cooperation targets described in claim 7, the relative posture of the space cooperation targets is obtained; Based on this relative position, space station rendezvous and docking, spacecraft rendezvous or space vehicle launch and recovery are carried out.

9. A terminal comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, it can be used to execute the method according to any one of claims 1 to 6, or to run the system according to any one of claim 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, it can be used to execute the method described in any one of claims 1 to 6, or to run the system described in any one of claim 7.

Images