CN113295171B - Monocular vision-based attitude estimation method for rotating rigid body spacecraft - Google Patents

Abstract

The invention discloses a monocular vision-based attitude estimation method for a rotary rigid spacecraft, which comprises the steps of establishing a known target rotary rigid spacecraft model and a coordinate system for describing the attitude of the spacecraft, carrying out median filtering and background separation on an image obtained by a monocular camera, detecting and marking all ellipses by using a rapid ellipse detection method, determining general functions of all the ellipses, carrying out radius matching on the marked ellipses and the known target model, solving by using a geometric method to obtain the normal direction and the central coordinate of a plane where the ellipses are located under a camera coordinate system, fitting all the ellipse central coordinates into a straight line by using a least square method, calculating the unit direction vector of the straight line, and obtaining the complete relative attitude of the target spacecraft relative to a tracked spacecraft according to the normal line and the central coordinate of the uniquely determined plane where the reference ellipses are located. The method has the advantages of small calculated amount, short consumed time and high accuracy, and can effectively estimate the relative attitude of the non-cooperative target spacecraft relative to the tracking spacecraft.

Description

Monocular vision-based attitude estimation method for rotating rigid body spacecraft

Technical Field

The invention belongs to the technical field of vision measurement of non-cooperative target spacecrafts in the space task technology, and relates to a method for estimating attitude of a rotating rigid body spacecraft based on monocular vision.

Background

Pose (relative position and attitude) estimation of the target spacecraft is an important issue for a wide range of spatial mission scenarios, such as Formation Flight (FF), on-orbit service (OOS), and Active Debris Removal (ADR). Relative pose estimation and navigation are essential in safe close range operations (from tens of meters to several meters), close range capture such as rendezvous and docking, and the like. In the most common non-cooperative target spacecraft (namely, no artificial marker or any communication link is arranged), the realization of the pose determination of the non-cooperative target spacecraft is an important basis for executing subsequent complex space tasks, so that the pose estimation problem research of the non-cooperative target spacecraft has important theoretical value and engineering significance.

For the pose estimation problem of the non-cooperative target, the patent CN108562274A adopts a binary square marker as a marker, casts a plurality of markers to the target spacecraft in the rendezvous and docking approach stage, and solves the relative pose between the target coordinate system and the camera coordinate system by identifying the position information of the satellite and rocket docking rings and the markers on the surface of the non-cooperative target. However, this method requires throwing a plurality of markers, which results in waste of resources, and is only suitable for the close proximity approach stage, and the identification of the markers increases the amount of calculation and brings estimation errors. In the patent CN111536981A, feature information such as frame corner points and docking rings of a non-cooperative target main body is extracted by a binocular camera, left and right image features are matched, three-dimensional coordinates of the matched features are restored, and finally, the position and posture of the non-cooperative target relative to the binocular camera are obtained by calculation, so as to obtain the relative pose between a coordinate system of the non-cooperative target and a coordinate system of the binocular camera. The method is restricted by the base line of the binocular camera, the observable distance is limited, the power consumption and the load are large, and the requirement on the load is improved.

Disclosure of Invention

The invention solves the problems: the method is used for obtaining the position and attitude information of the non-cooperative target spacecraft relative to the tracking spacecraft and establishing a foundation for the subsequent complex space task. The invention realizes a monocular vision system based on low power and low quality requirements, and under the condition of not depending on additional measurement information, the invention fits the straight line where the centers of a plurality of circles of the rotating rigid spacecraft are positioned by the least square method to determine the normal direction and the central position of the support plane of the spacecraft, thereby realizing the accurate measurement of the relative pose of the non-cooperative target spacecraft, having small calculated amount and high accuracy, and providing higher value for practical engineering application.

The invention provides a monocular vision-based attitude estimation method for a rotating rigid body spacecraft, which comprises the following steps:

s1: establishing a known target rotating rigid body spacecraft 3D model, and simultaneously establishing a coordinate system for describing the posture of a spacecraft, wherein the coordinate system comprises a pixel coordinate system, an image coordinate system, a camera coordinate system and a target rotating rigid body spacecraft body coordinate system; calculating a conversion relation among the pixel coordinate system, the image coordinate system, the camera coordinate system and the target rotating rigid body spacecraft body coordinate system to obtain a coordinate conversion matrix;

s2: calculating pixel coordinates of an image obtained by a monocular camera on a tracking spacecraft in a pixel coordinate system, and performing median filtering to obtain a filtered image;

s3: processing the image after median filtering, and separating the spacecraft from the background by using a weak gradient elimination method to obtain an image after background separation;

s4: processing the image after background separation, detecting all ellipses in the image after background separation by using a rapid ellipse detection method, marking the ellipses according to the sequence 1,2 and … n, and determining general functions of all the ellipses;

s5: matching the radii of the ellipses of all the marks with the known 3D model of the target rotating rigid body spacecraft, and solving general functions of all the ellipses by using a geometric method to obtain normal vectors and central coordinates of the plane where each ellipse is located under a camera coordinate system;

s6: all the center coordinates (x) _i ,y _i ,z _i ) Fitting i belongs to {1,2.. n } to form a straight line, calculating to obtain a unit direction vector of the straight line, wherein i represents the ith mark ellipse;

s7: according to the characteristic that the centers of all ellipses are on one straight line, and the straight line is the normal line of the plane where the ellipses are located, the normal vector and the central coordinate of the plane where the ellipses are located are uniquely determined, the plane where the first ellipse is located is used as a reference, the rotation matrix of the plane where the reference ellipse is located relative to the tracking spacecraft is calculated, and the rotation matrix is used as the estimation of the attitude of the rotating rigid-body spacecraft.

In step S1, a known target rotating rigid body spacecraft 3D model is established, and a coordinate system describing the posture of the spacecraft is established, where the coordinate system includes a pixel coordinate system, an image coordinate system, a camera coordinate system, and a target rotating rigid body spacecraft body coordinate system; and calculating a conversion relation among the pixel coordinate system, the image coordinate system, the camera coordinate system and the target rotating rigid body spacecraft body coordinate system to obtain a coordinate conversion matrix, which specifically comprises the following steps:

pixel coordinate system O _uv -uv: to track the vertex O of the image obtained by a monocular camera on a spacecraft _uv The point is an original point, the horizontal coordinate u axis is a row where the image is located, and the vertical coordinate v axis is a column where the image is located;

image coordinate system O _xy -xy: at the center O of the image _xy As an origin, the x-axis is parallel to the u-axis and the y-axis is parallel to the v-axis of the pixel coordinate system;

camera coordinate system O _c -X _c Y _c Z _c : with camera optical center O _c Is an origin, Z _c Axial in the direction of the optical axis, X _c The axis being parallel to the x-axis, Y, of the image coordinate system _c The axis is parallel to the y-axis of the image coordinate system;

with the mass center O of the target spacecraft body _w : establishing a target spacecraft body coordinate system O for an origin _w -X _w Y _w Z _w The direction of the outward normal of the plane of the ellipse is taken as Z _w Axis X in the direction perpendicular to the normal of the plane of the ellipse _w Axis, Y _w The axes are respectively connected with X _w Axis and Z _w The axes are vertical and form a right-handed screw system;

establishing a coordinate system O on the plane of the ellipse _D -X _D Y _D Z _D At the center O of the plane _D Is an origin, X _D Axis parallel to X of body coordinate system _w Axis, Y _D Y with axis parallel to body coordinate system of target spacecraft _w Axis, Z _D Z with axis parallel to body coordinate system of target spacecraft _w A shaft.

In the step S2, calculating a pixel coordinate of an image obtained by tracking a monocular camera on a spacecraft in a pixel coordinate system, and performing median filtering to obtain a filtered image, specifically including:

s21: converting an image obtained by a monocular camera on a tracked spacecraft into a gray image;

s22: selecting a sliding template (generally 3 multiplied by 3), aligning the first three rows and the first three columns of pixel points of the gray image, sequencing all the pixel points in the sliding template according to the pixel values to generate a two-dimensional data sequence which monotonically rises or monotonically falls, and selecting a median value in the two-dimensional data sequence to replace the pixel value of a central pixel point in the sliding template;

s23: the whole sliding template translates a row of pixel points along the direction of the u axis of the pixel coordinate system, and the step S22 is repeated until the scanning of all the pixels of the row is completed;

s24: and moving the pixels of the whole sliding template downwards by one line, repeating the step S22 and the step S23, scanning the next line, and finally obtaining the image after median filtering.

In step S3, the image after the median filtering is processed, and the spacecraft is separated from the background by using the weak gradient elimination method to obtain an image after the background separation, which specifically includes:

s31: calculating image gradient by using a Priwitt operator, and calculating the gradient of the image I in the horizontal direction and the vertical direction by using 2 convolution kernels with the window size of 3 multiplied by 3 respectively;

in the formula, G _x ,G _y Respectively representing the horizontal and vertical gradients of the image, and representing a two-dimensional convolution operator;

s32: taking the root mean square of the gradient values in the horizontal direction and the vertical direction as the integral gradient of the image;

wherein G (u, v) represents an image gradient value at an image coordinate of (u, v);

s33: arranging gradient values G (u, v) of all pixels in the image in an ascending order, uniformly dividing the gradient values into a plurality of groups (100 groups are selected in the invention), and approximating the histogram form to an exponential probability density function according to the frequency statistics of each group in a histogram form

Wherein x represents an image gradient value, and lambda is obtained by fitting histogram data by using a formula (3);

s34: substituting the percentage of the weak gradient value pixel quantity in the total pixel quantity and lambda obtained by the formula (3) into the exponential distribution function formula (4), calculating the corresponding gradient segmentation threshold value, and setting the pixel values smaller than the gradient segmentation threshold value to be 0

The step S4, processing the image after background separation, detecting all ellipses in the image after background separation by using a fast ellipse detection method, labeling the ellipses in the sequence 1,2, … n, and determining general functions of all the ellipses, specifically including:

processing the image after median filtering by using a rapid ellipse detection method to obtain five parameters (x) of a series of ellipses _0i ,y _0i ,a _i ,b _i ,θ _i ) I ∈ {1,2.. n }, where (x) _0i ,y _0i ) Represents the center position of the ellipse, a _i Semi-major axis of the ellipse, b _i Semi-minor axis, theta, representing an ellipse _i Representing the angle by which the long axis is rotated from the x-axis of the image coordinate system; from the five parameters of the ellipse, a general function for determining the ellipse is as follows:

Au ² +Bv ² +Cuv+Du+Ev+F＝0 (5)

wherein A, B, C, D, E and F are parameters of an elliptic general function;

step S5, performing radius matching on the ellipses of all the marks and the known target rotational rigid body spacecraft 3D model, and solving general functions of all the ellipses by using a geometric method to obtain normal vectors and center coordinates of a plane where each ellipse is located in the camera coordinate system, specifically including:

s51: matching the marked ellipse with the known target spacecraft 3D model from Z _w Matching in the positive direction of the axis to obtain the radius r of each matched circle _i The formula (5) is rewritten into an algebraic form:

[u v 1]Q[u v 1] ^T ＝0 (7)

wherein,

the point is represented as P in the camera coordinate system _c ＝(X _c ,Y _c ,Z _c ) ^T Expressed as (u, v) in the pixel coordinate system ^T Corresponding homogeneous coordinate is expressed as (u, v,1) ^T The following relationship is satisfied:

X _c ,Y _c ,Z _c respectively representing points along X in the camera coordinate system _c Axis, Y _c Axis and Z _c Distance of the shaft; m _ins The internal parameter matrix of the monocular camera is obtained; will be given by the formula (7) Turning to a camera coordinate system, and obtaining an oblique elliptic cone gamma equation under the camera coordinate system as follows:

[X _c Y _c Z _c ]C _Q [X _c Y _c Z _c ] ^T ＝0 (10)

wherein,

establishing a coordinate system O at the origin of the camera coordinate system _c -X 'Y' Z ', the Z' axis being parallel to the normal of the plane of the ellipse, the oblique elliptic cone Γ becoming a positive elliptic cone; coordinate system O _c -X ' Y ' Z ' and camera coordinate system O _c -X _c Y _c Z _c There is only a rotational transformation; transforming the matrix P by rotation, C _Q Conversion to diagonal matrix:

P ^T C _Q P＝diag(λ ₁ ,λ ₂ ,λ ₃ ) (12)

wherein λ is ₁ 、λ ₂ And λ ₃ Is C _Q A characteristic value of (a), and ₁ ≥λ ₂ ＞0＞λ ₃ (ii) a Reckoning the coordinate System O _c -center coordinate O 'of the plane in which the ith ellipse is located under X' Y 'Z' _i And normal vector n' _i Comprises the following steps:

wherein i belongs to {1,2.. n }, r _i Representing the radius of each circle after matching with the known 3D model of the target spacecraft;

the center coordinate O 'of the plane of the ellipse' _i And normal vector n' _i Turning to the camera coordinate system:

wherein,

representing the center coordinate of the plane where the ith ellipse is located under the camera coordinate system;

a normal vector representing the plane of the ith ellipse in the camera coordinate system;

s52: comparing the ellipse to be marked with the known 3D model of the target spacecraft from Z _w Matching in the opposite direction, and repeating the step S51 to obtain the radius r of each matched circle _i Calculating Z _w The axes are matched with the center coordinates and normal vectors of the plane where the series of ellipses are located in the lower camera coordinate system in the opposite direction.

The step S6, converting all the center coordinates (x) _i ,y _i ,z _i ) I belongs to {1,2.. n } and is fitted into a straight line, a unit direction vector where the straight line is located is obtained through calculation, i represents the ith mark ellipse, and the method specifically comprises the following steps:

the simplified form of the line l is:

wherein a, b, c and d are parameters of a straight line l, and the matrix form converted from (15) is as follows:

all center coordinates

Satisfies the following conditions:

thus:

the direction vector of the straight line l is obtained as N ═ a, c,1, and N is converted into a unit direction vector

Step S7, according to the feature that the centers of all ellipses are on a straight line, and the straight line is the normal of the plane where the ellipses are located, uniquely determining the normal vector and the central coordinate of the plane where the ellipses are located, taking the plane where the first ellipse is located as the reference, calculating the rotation matrix of the plane where the reference ellipse is located with respect to the tracked spacecraft, where the rotation matrix is used as the estimation of the attitude of the rotating rigid-body spacecraft, specifically including:

s71: calculating the vector n in the direction of the normal line l of the ellipse and the two normal vectors of the plane of the reference ellipse obtained by S5

Angle therebetween

S72: uniquely determining a normal vector of a plane where a reference ellipse is located and a corresponding central coordinate thereof according to the following principle;

wherein ε < 5 ° represents an angle threshold;

s73: calculating a displacement vector t of the target spacecraft relative to the tracking spacecraft according to the center coordinate of the plane where the uniquely determined reference ellipse is located:

t＝(O _x ,O _y ,O _z ) ^T (21)

wherein (O) _x ,O _y ,O _z ) The center coordinates of the plane where the uniquely determined reference ellipse is located are represented;

the normal vector of the plane of the uniquely determined reference ellipse

As Z _D Axial direction vector

Calculating a vector perpendicular thereto as X _D Axial direction vector

Thereby calculating a rotation matrix R of the target spacecraft relative to the tracking spacecraft:

wherein,

representing the normal vector of the plane in which the uniquely determined reference ellipse lies.

The normal vector of the plane of the uniquely determined reference ellipse

As Z _D Axial direction vector

Calculating a vector perpendicular thereto as X _D Axial direction vector

Thereby calculating a rotation matrix R of the target spacecraft relative to the tracking spacecraft:

wherein,

representing the normal vector of the plane in which the uniquely determined reference ellipse lies.

In order to facilitate the analysis of the error determined by the relative attitude, the rotation matrix R is written into the form of Euler angle, and the target spacecraft is made to rotate around X _w The angle of rotation of the shaft being pitch phi, about Y _w The angle of rotation of the shaft being yaw angle

Around Z _w The rotation angle of the shaft is a roll angle gamma, and the rotation matrix R is in the form of Euler angle according to X _w 、Y _w 、Z _w The rotation sequence of (a) is expressed as:

from equation (23), the euler angle can be calculated, expressed as:

wherein R is _ij (i, j e (1,2,3)) represents the i-th row and j-th column element of the rotation matrix R.

Compared with the prior art, the invention has the advantages that:

(1) compared with the existing spacecraft pose estimation method based on binocular vision, the spacecraft pose estimation method based on monocular vision designed by the invention is not influenced by baseline constraint, has lower hardware complexity and cost, lower power consumption and load, reduces the requirement on load, can ensure that the pose is determined quickly under the low-power and quality requirements, and better meets the actual engineering requirement.

(2) Compared with the existing monocular vision spacecraft pose estimation method based on the projection marker, the monocular vision-based spacecraft pose estimation method designed by the invention does not need to additionally project and identify the marker, does not cause resource waste, is suitable for spacecrafts with longer distance and without markers, has small calculated amount, short consumed time and high accuracy, can effectively estimate the relative posture of the non-cooperative target spacecraft relative to the tracking spacecraft, and has good engineering value.

Drawings

FIG. 1 is a flow chart of a method for estimating attitude of a rotating rigid body spacecraft based on monocular vision according to the present invention;

fig. 2 is a schematic diagram of a coordinate system for describing the attitude of a spacecraft, which is established in embodiment 1 of the invention;

FIG. 3 is a normal vector diagram estimated from the centers of all detected ellipses in the target spacecraft in embodiment 1 of the present invention;

FIG. 4 is a diagram of an absolute error of an ellipsometric vector in a target spacecraft in embodiment 1 of the present invention;

fig. 5 is a diagram of absolute error of a displacement vector of a target spacecraft with respect to a tracking spacecraft in embodiment 1 of the present invention;

FIG. 6 is a diagram of relative errors of a target spacecraft with respect to a displacement vector of a tracking spacecraft in embodiment 1 of the present invention;

fig. 7 is a diagram of an estimation error of the euler angle of the target spacecraft with respect to the tracking spacecraft in embodiment 1 of the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only illustrative and are not intended to limit the present invention.

As shown in fig. 1, the method for estimating the attitude of a rotating rigid body spacecraft based on monocular vision of the present invention comprises the following steps:

s1: establishing a known target rotating rigid body spacecraft 3D model, and simultaneously establishing a coordinate system for describing the posture of a spacecraft, wherein the coordinate system comprises a pixel coordinate system, an image coordinate system, a camera coordinate system and a target rotating rigid body spacecraft body coordinate system; calculating a conversion relation among the pixel coordinate system, the image coordinate system, the camera coordinate system and the target rotating rigid body spacecraft body coordinate system to obtain a coordinate conversion matrix;

s2: calculating pixel coordinates of an image obtained by a monocular camera on a tracking spacecraft in a pixel coordinate system, and performing median filtering to obtain a filtered image;

s3: processing the image after median filtering, and separating the spacecraft from the background by using a weak gradient elimination method to obtain an image after background separation;

s4: processing the image after background separation, detecting all ellipses in the image after background separation by using a rapid ellipse detection method, marking the ellipses according to the sequence 1,2 and … n, and determining general functions of all the ellipses;

s5: matching the radii of the ellipses of all the marks with the known 3D model of the target rotating rigid body spacecraft, and solving general functions of all the ellipses by using a geometric method to obtain normal vectors and central coordinates of the plane where each ellipse is located under a camera coordinate system;

s6: all the center coordinates (x) _i ,y _i ,z _i ) Fitting i belongs to {1,2.. n } to form a straight line, calculating to obtain a unit direction vector of the straight line, wherein i represents the ith mark ellipse;

s7: according to the characteristic that the centers of all ellipses are on one straight line, and the straight line is the normal line of the plane where the ellipses are located, the normal vector and the central coordinate of the plane where the ellipses are located are uniquely determined, the plane where the first ellipse is located is used as a reference, the rotation matrix of the plane where the reference ellipse is located relative to the tracking spacecraft is calculated, and the rotation matrix is used as the estimation of the attitude of the rotating rigid-body spacecraft.

The following describes in detail a specific implementation of the aforementioned monocular vision-based rigid body spacecraft attitude estimation method according to the present invention with a specific embodiment.

Example 1:

firstly, establishing a known target rotating rigid body spacecraft 3D model, and simultaneously establishing a coordinate system for describing the posture of a spacecraft, wherein the coordinate system comprises a pixel coordinate system, an image coordinate system, a camera coordinate system and a target rotating rigid body spacecraft body coordinate system; and calculating a conversion relation among the pixel coordinate system, the image coordinate system, the camera coordinate system and the target rotating rigid body spacecraft body coordinate system to obtain a coordinate conversion matrix. The method is realized by the following specific steps:

as shown in fig. 2, each coordinate system is established as follows: pixel coordinate system O _uv -uv: to track the vertex O of the image obtained by a monocular camera on a spacecraft _uv The point is an original point, the horizontal coordinate u axis is a row where the image is located, and the vertical coordinate v axis is a column where the image is located; image coordinate system O _xy -xy: at the center O of the image _xy As an origin, the x-axis is parallel to the u-axis and the y-axis is parallel to the v-axis of the pixel coordinate system; camera coordinate system O _c -X _c Y _c Z _c : with camera optical center O _c Is an origin, Z _c Axial in the direction of the optical axis, X _c The axis being parallel to the x-axis, Y, of the image coordinate system _c The axis is parallel to the y-axis of the image coordinate system; with the mass center O of the target spacecraft body _w : establishing a target spacecraft body coordinate system O for an origin _w -X _w Y _w Z _w The direction of the outward normal of the plane of the ellipse is taken as Z _w Axis X in the direction perpendicular to the normal of the plane of the ellipse _w Axis, Y _w The axes are respectively connected with X _w Axis and Z _w The axes are vertical and form a right-handed screw system; establishing a coordinate system O on the plane of the ellipse _D -X _D Y _D Z _D At the center O of the plane _D Is an origin, X _D Axis parallel to X of body coordinate system _w Axis, Y _D Y with axis parallel to body coordinate system of target spacecraft _w Axis, Z _D Z with axis parallel to body coordinate system of target spacecraft _w A shaft.

And secondly, calculating the pixel coordinates of an image obtained by a monocular camera on the tracking spacecraft in a pixel coordinate system, and performing median filtering to obtain a filtered image. The method is realized by the following specific steps:

(1) converting an image obtained by a monocular camera on a tracked spacecraft into a gray image;

(2) selecting a sliding template, aligning the first three rows and the first three columns of pixel points of the gray level image, sequencing all the pixel points in the sliding template according to the pixel values to generate a two-dimensional data sequence which monotonically rises or monotonically falls, and selecting a median value in the two-dimensional data sequence to replace the pixel value of a central pixel point in the sliding template;

(3) the whole sliding template translates a row of pixel points along the direction of the u axis of the pixel coordinate system, and the step S22 is repeated until the scanning of all the pixels of the row is completed;

(4) and moving the pixels of the whole sliding template downwards by one line, repeating the step S22 and the step S23, scanning the next line, and finally obtaining the image after median filtering.

And thirdly, processing the image after median filtering, and separating the spacecraft from the background by using a weak gradient elimination method to obtain the image after background separation. The method is realized by the following specific steps:

(1) calculating image gradient by using a Priwit operator, and calculating the gradient of the image I in the horizontal direction and the vertical direction by using 2 convolution kernels with the window size of 3 multiplied by 3 respectively;

in the formula, G _x ,G _y Representing the image horizontal and vertical gradients, respectively, representing the two-dimensional convolution operator.

(2) Taking the root mean square of the gradient values in the horizontal direction and the vertical direction as the integral gradient of the image;

in the formula, G (u, v) represents an image gradient at image coordinates (u, v).

(3) Arranging gradient values G (u, v) of all pixels in the image in an ascending order, uniformly dividing the gradient values into 100 groups, counting the frequency of each group as a histogram, and approximating the histogram as an exponential probability density function

Wherein x represents an image gradient value, and lambda is obtained by fitting histogram data by using a formula (3);

(4) the percentage of the weak gradient value pixel amount to the total pixel amount and λ obtained in (3) are substituted into the exponential distribution function (4), so that the corresponding gradient division threshold value can be calculated, and all the pixel values smaller than the gradient division threshold value are set to 0.

And fourthly, processing the image after the background separation, detecting all ellipses in the image after the background separation by using a rapid ellipse detection method, marking the ellipses according to the sequence 1,2 and … n, and determining general functions of all the ellipses. The method is realized by the following specific steps:

processing the image after median filtering by using a rapid ellipse detection method to obtain five parameters (x) of a series of ellipses _0i ,y _0i ,a _i ,b _i ,θ _i ) I ∈ {1,2.. n }, where (x) _0i ,y _0i ) Represents the center position of the ellipse, a _i Semi-major axis of the ellipse, b _i Semi-minor axis, theta, representing an ellipse _i Representing the angle through which the major axis is rotated from the x-axis of the image coordinate system; from the five parameters of the ellipse, a general function for determining the ellipse is as follows:

Au ² +Bv ² +Cuv+Du+Ev+F＝0 (5)

wherein, A, B, C, D, E and F are parameters of an elliptic general function.

And fifthly, matching the marked ellipse with the known target spacecraft 3D model from two directions respectively, and solving general functions of all the ellipses by using a geometric method to obtain normal vectors and central coordinates of the plane where each ellipse is located under the camera coordinate system.

(1) Matching the marked ellipse with the known target spacecraft 3D model from Z _w Matching in the positive direction of the axis to obtain the radius r of each matched circle _i . Rewrite equation (5) to algebraic form:

[u v 1]Q[u v 1] ^T ＝0 (7)

wherein,

due to the fact that

Wherein M is _ins The internal parameter matrix of the monocular camera is obtained; and (3) converting the formula (7) into a camera coordinate system to obtain an oblique elliptic cone gamma equation under the camera coordinate system as follows:

[X _c Y _c Z _c ]C _Q [X _c Y _c Z _c ] ^T ＝0 (10)

wherein,

establishing a coordinate system O at the origin of the camera coordinate system _c -X 'Y' Z ', the Z' axis being parallel to the normal of the plane of the ellipse, the oblique elliptic cone Γ becoming a positive elliptic cone; coordinate system O _c -X ' Y ' Z ' and camera coordinate system O _c -X _c Y _c Z _c There is only a rotational transformation; by passingRotating transformation matrix P, C _Q Conversion to diagonal matrix:

P ^T C _Q P＝diag(λ ₁ ,λ ₂ ,λ ₃ ) (12)

wherein λ is ₁ 、λ ₂ And λ ₃ Is C _Q Characteristic value of

And λ ₁ ≥λ ₂ ＞0＞λ ₃ (ii) a Reckoning the coordinate system O _c -center coordinate O 'of the plane of the ith ellipse under X' Y 'Z' _i And normal vector n' _i Comprises the following steps:

wherein i belongs to {1,2.. n }, r _i Representing the radius of each circle after matching with the known 3D model of the target spacecraft;

the center coordinate O 'of the plane of the ellipse' _i And normal vector n' _i Turning to the camera coordinate system:

wherein,

representing the center coordinate of the plane where the ith ellipse is located under the camera coordinate system;

and the normal vector of the plane where the ith ellipse is located in the camera coordinate system is represented.

(2) Matching the marked ellipse with the known target spacecraft 3D model from Z _w Matching in the opposite direction of the axis, and repeating the step (1) to obtain the radius r of each matched circle _i Calculating Z _w The axes are matched with the center of the plane of a series of ellipses in the lower camera coordinate system in the opposite directionThe normal vectors are summed.

Sixthly, all the center coordinates (x) _i ,y _i ,z _i ) And i belongs to {1,2.. n } and is fitted into a straight line, a unit direction vector where the straight line is located is obtained through calculation, and i represents the ith mark ellipse. The method is realized by the following steps:

the simplified form of the line l is:

wherein a, b, c and d are parameters of a straight line l, and the matrix form converted from (15) is as follows:

all center coordinates

Satisfies the following conditions:

thus:

this gives (a, c,1) the direction vector of the straight line l, and converts N into a unit direction vector

And seventhly, according to the characteristic that the centers of all ellipses are in one straight line which is the normal of the plane in which the ellipses are located, uniquely determining a normal vector and a central coordinate of the plane in which the ellipses are located, taking the plane in which the first ellipse is located as a reference, calculating a rotation matrix of the plane in which the reference ellipse is located relative to the tracking spacecraft, and taking the rotation matrix as estimation of the attitude of the rotating rigid body spacecraft. The method is realized by the following specific steps:

(1) calculating the vector n in the direction of the normal line l of the ellipse and the two normal vectors of the plane of the reference ellipse obtained in the fifth step

Angle therebetween

(2) And uniquely determining a normal vector of a plane where the reference ellipse is located and a corresponding central coordinate thereof according to the following principle.

Wherein ε < 5 ° represents an angle threshold;

(3) calculating a displacement vector t of the target spacecraft relative to the tracking spacecraft according to the center coordinate of the plane where the uniquely determined reference ellipse is located:

t＝(O _x ,O _y ,O _z ) ^T (21)

wherein (O) _x ,O _y ,O _z ) The center coordinates of the plane where the uniquely determined reference ellipse is located are represented;

the normal vector of the plane of the uniquely determined reference ellipse

As Z _D Axial direction vector

Calculating a vector perpendicular thereto as X _D Axial direction vector

Thereby calculating a rotation matrix R of the target spacecraft relative to the tracking spacecraft:

wherein,

representing the normal vector of the plane in which the uniquely determined reference ellipse lies.

In order to facilitate the analysis of the error determined by the relative attitude, the rotation matrix R is written into the form of Euler angle, and the target spacecraft is made to rotate around X _w The angle of rotation of the shaft being pitch phi, about Y _w The angle of rotation of the shaft being yaw angle

Around Z _w The angle of the axis rotation is a roll angle gamma, and the rotation matrix R is in the form of Euler angle according to X _w 、Y _w 、Z _w The rotation sequence of (a) is expressed as:

from equation (23), the euler angle can be calculated, expressed as:

wherein R is _ij (i, j e (1,2,3)) represents the i-th row and j-th column element of the rotation matrix R.

The precision of the method for estimating the attitude of the rotating rigid body spacecraft based on monocular vision provided by the embodiment 1 of the invention is detected as follows. FIG. 1 is a flow chart of the steps of the present invention. FIG. 2 depicts a target rotating rigid body spacecraftThe model comprises a pixel coordinate system, an image coordinate system, a camera coordinate system, a plane coordinate system where a datum ellipse is located and a target spacecraft body coordinate system. Fig. 3 depicts the direction vectors, estimated normal vectors and true normal vectors of all the detected ellipse centers in the target spacecraft from the straight lines where the centers of the ellipses are located by using the least square method when the target spacecraft is 4 meters away from the tracking spacecraft, and fig. 4 shows the absolute error curve between the estimated normal vectors and the true normal vectors, so that the errors are all within 0.4 degrees. Fig. 5 and 6 respectively show the absolute error and relative error curve of the target spacecraft relative to the tracking spacecraft, the absolute error is within 10cm, and the relative error is less than 1.2%, so that the relative position of the target spacecraft relative to the tracking spacecraft is recovered. FIG. 7 shows the Euler angle error of the target spacecraft relative to the tracking spacecraft, with the upper diagram representing the pitch angle psi error, the middle diagram representing the roll angle gamma error, and the lower diagram representing the yaw angle

The error, all euler angle errors in the graph are very small. All angle and position errors in fig. 3-7 decrease as the monocular camera approaches the target spacecraft because the resolution of the objects in the image increases and the error in feature extraction decreases as the distance between the monocular camera and the target spacecraft decreases. The simulation results fully show that the method for estimating the attitude of the rotating rigid body spacecraft based on monocular vision, provided by the embodiment of the invention, is feasible and effective, has small calculated amount and high accuracy, and can realize effective estimation of the attitude of the non-cooperative target spacecraft.

It will be apparent to those skilled in the art that various changes and modifications may be made in the present invention without departing from the spirit and scope of the invention. Thus, if such modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include such modifications and variations.

Claims (8)

1. A method for estimating the attitude of a rotating rigid body spacecraft based on monocular vision is characterized by comprising the following steps:

s1: establishing a known target rotating rigid body spacecraft 3D model, and simultaneously establishing a coordinate system for describing the posture of a spacecraft, wherein the coordinate system comprises a pixel coordinate system, an image coordinate system, a camera coordinate system and a target rotating rigid body spacecraft body coordinate system; calculating a conversion relation among the pixel coordinate system, the image coordinate system, the camera coordinate system and the target rotating rigid body spacecraft body coordinate system to obtain a coordinate conversion matrix;

s2: calculating pixel coordinates of an image obtained by a monocular camera on a tracking spacecraft in a pixel coordinate system, and performing median filtering to obtain a filtered image;

s3: processing the image after median filtering, and separating the spacecraft from the background by using a weak gradient elimination method to obtain an image after background separation;

s4: processing the image after background separation, detecting all ellipses in the image after background separation by using a rapid ellipse detection method, marking the ellipses according to the sequence 1,2 and … n, and determining general functions of all the ellipses;

s5: matching the radii of the ellipses of all the marks with the known 3D model of the target rotating rigid body spacecraft, and solving general functions of all the ellipses by using a geometric method to obtain normal vectors and central coordinates of the plane where each ellipse is located under a camera coordinate system;

s6: all the center coordinates (x) _i ,y _i ,z _i ) Fitting i belongs to {1,2.. n } to form a straight line, calculating to obtain a unit direction vector of the straight line, wherein i represents the ith mark ellipse;

s7: according to the characteristic that the centers of all ellipses are on one straight line, and the straight line is the normal line of the plane where the ellipses are located, the normal vector and the central coordinate of the plane where the ellipses are located are uniquely determined, the plane where the first ellipse is located is used as a reference, the rotation matrix of the plane where the reference ellipse is located relative to the tracking spacecraft is calculated, and the rotation matrix is used as the estimation of the attitude of the rotating rigid-body spacecraft.

2. A monocular vision based rotating rigid body spacecraft attitude estimation method according to claim 1, wherein in step S1, a known target rotating rigid body spacecraft 3D model is established, along with a coordinate system describing spacecraft attitude, the coordinate system comprising a pixel coordinate system, an image coordinate system, a camera coordinate system, and a target rotating rigid body spacecraft body coordinate system; and calculating a conversion relation among the pixel coordinate system, the image coordinate system, the camera coordinate system and the target rotating rigid body spacecraft body coordinate system to obtain a coordinate conversion matrix, which specifically comprises the following steps:

pixel coordinate system O _uv -uv: to track the vertex O of the image obtained by a monocular camera on a spacecraft _uv The point is an original point, the horizontal coordinate u axis is a row where the image is located, and the vertical coordinate v axis is a column where the image is located;

image coordinate system O _xy -xy: at the center O of the image _xy As an origin, the x-axis is parallel to the u-axis and the y-axis is parallel to the v-axis of the pixel coordinate system;

camera coordinate system O _c -X _c Y _c Z _c : with camera optical center O _c Is an origin, Z _c Axial in the direction of the optical axis, X _c The axis being parallel to the x-axis, Y, of the image coordinate system _c The axis is parallel to the y-axis of the image coordinate system;

with the mass center O of the target spacecraft body _w Establishing a target spacecraft body coordinate system O for an origin _w -X _w Y _w Z _w The direction of the outward normal of the plane of the ellipse is taken as Z _w Axis X in the direction perpendicular to the normal of the plane of the ellipse _w Axis, Y _w The axes are respectively connected with X _w Axis and Z _w The axes are vertical and form a right-handed screw system;

establishing a coordinate system O on the plane of the ellipse _D -X _D Y _D Z _D At the center O of the plane _D Is an origin, X _D Axis parallel to X of body coordinate system _w Axis, Y _D Y with axis parallel to body coordinate system of target spacecraft _w Axis, Z _D Z with axis parallel to body coordinate system of target spacecraft _w A shaft.

3. The method for estimating pose of rotating rigid body spacecraft based on monocular vision according to claim 1, wherein in step S2, calculating pixel coordinates of an image obtained by tracking a monocular camera on the spacecraft in a pixel coordinate system, and performing median filtering to obtain a filtered image, specifically comprising:

s21: converting an image obtained by a monocular camera on a tracked spacecraft into a gray image;

s22: selecting a sliding template, aligning the first three rows and the first three columns of pixel points of the gray level image, sequencing all the pixel points in the sliding template according to the size of the pixel values to generate a monotonously-rising or monotonously-falling two-dimensional data sequence, and selecting a median value in the two-dimensional data sequence to replace the pixel value of a central pixel point in the sliding template;

s23: the whole sliding template translates a row of pixel points along the direction of the u axis of the pixel coordinate system, and the step S22 is repeated until the scanning of all the pixels of the row is completed;

s24: and moving the pixels of the whole sliding template downwards by one line, repeating the step S22 and the step S23, scanning the next line, and finally obtaining the image after median filtering.

4. The method for estimating pose of rotating rigid body spacecraft based on monocular vision according to claim 1, wherein in step S3, the processing is performed on the image after median filtering, and the spacecraft is separated from the background by weak gradient elimination, so as to obtain the image after background separation, specifically comprising:

s31: calculating image gradient by using a Priwit operator, and calculating the gradient of the image I in the horizontal direction and the vertical direction by using 2 convolution kernels with the window size of 3 multiplied by 3 respectively;

in the formula, G _x ,G _y Respectively representing the horizontal and vertical gradients of the image, and representing a two-dimensional convolution operator;

s32: taking the root mean square of the gradient values in the horizontal direction and the vertical direction as the integral gradient of the image;

wherein G (u, v) represents an image gradient value at an image coordinate of (u, v);

s33: arranging gradient values G (u, v) of all pixels in the image in an ascending order, uniformly dividing the gradient values into a plurality of groups, counting the frequency of each group as a histogram form, and approximating the histogram form as an exponential probability density function

Wherein x represents an image gradient value, and lambda is obtained by fitting histogram data by using a formula (3);

s34: substituting the percentage of the weak gradient value pixel quantity in the total pixel quantity and lambda obtained by the formula (3) into an exponential distribution function formula (4), calculating a corresponding gradient segmentation threshold value, and setting the pixel values which are less than or equal to the gradient segmentation threshold value as 0;

5. the method for estimating pose of rotating rigid body spacecraft based on monocular vision as claimed in claim 1, wherein step S4, the image after background separation is processed, all ellipses in the image after background separation are detected by using fast ellipse detection method, labeled by 1,2, … n in sequence, and general functions of all ellipses are determined, specifically comprising:

processing the image after median filtering by using a rapid ellipse detection method to obtain five parameters (x) of a series of ellipses _0i ,y _0i ,a _i ,b _i ,θ _i ) I ∈ {1,2.. n }, where (x) _0i ,y _0i ) Represents the center position of the ellipse, a _i Semi-major axis of the ellipse, b _i Semi-minor axis, theta, representing an ellipse _i Representing the angle through which the major axis is rotated from the x-axis of the image coordinate system; from the five parameters of the ellipse, a general function for determining the ellipse is as follows:

Au ² +Bv ² +Cuv+Du+Ev+F＝0 (5)

wherein A, B, C, D, E and F are parameters of an elliptic general function;

6. the method for estimating pose of rotating rigid body spacecraft based on monocular vision according to claim 5, wherein the step S5 is to match radii of all marked ellipses with known 3D model of target rotating rigid body spacecraft, and solve general functions of all ellipses by using a geometric method to obtain normal vector and center coordinate of a plane where each ellipse is located under a camera coordinate system, and specifically comprises:

s51: matching the marked ellipse with the known target spacecraft 3D model from Z _w Matching in the positive direction of the axis to obtain the radius r of each matched circle _i The formula (5) is rewritten into an algebraic form:

[u v 1]Q[u v 1] ^T ＝0 (7)

wherein,

the point is represented as P in the camera coordinate system _c ＝(X _c ,Y _c ,Z _c ) ^T Expressed as (u, v) in the pixel coordinate system ^T Corresponding homogeneous coordinate is expressed as (u, v,1) ^T The following relationship is satisfied:

X _c ,Y _c ,Z _c respectively representing points along X in the camera coordinate system _c Axis, Y _c Axis and Z _c Distance of the shaft; m _ins The internal parameter matrix of the monocular camera is obtained; converting the formula (7) into a camera coordinate system to obtain an oblique elliptic cone gamma equation under the camera coordinate system as follows:

[X _c Y _c Z _c ]C _Q [X _c Y _c Z _c ] ^T ＝0 (10)

wherein,

establishing a coordinate system O at the origin of the camera coordinate system _c -X 'Y' Z ', the Z' axis being parallel to the normal of the plane of the ellipse, the oblique elliptic cone Γ becoming a positive elliptic cone; coordinate system O _c -X ' Y ' Z ' and camera coordinate System O _c -X _c Y _c Z _c There is only a rotational transformation; transforming the matrix P by rotation, C _Q Conversion to diagonal matrix:

P ^T C _Q P＝diag(λ ₁ ,λ ₂ ,λ ₃ ) (12)

wherein λ is ₁ 、λ ₂ And λ ₃ Is C _Q Characteristic value of (a), and ₁ ≥λ ₂ ＞0＞λ ₃ (ii) a Coordinate system O _c -center coordinate O 'of the plane of the ith ellipse under X' Y 'Z' _i And normal vector n' _i Comprises the following steps:

wherein i belongs to {1,2.. n }, r _i Showing and knowing the purposeMarking the radius of each circle after the spacecraft 3D model is matched;

the center coordinate O 'of the plane of the ellipse' _i And normal vector n' _i Turning to the camera coordinate system:

wherein,

representing the center coordinate of the plane where the ith ellipse is located under the camera coordinate system;

a normal vector representing the plane of the ith ellipse in the camera coordinate system;

s52: matching the marked ellipse with the known target spacecraft 3D model from Z _w Matching in the opposite direction, and repeating the step S51 to obtain the radius r of each matched circle _i Calculating Z _w The axes are matched with the center coordinates and normal vectors of the plane where the series of ellipses are located in the lower camera coordinate system in the opposite direction.

7. A method for estimating pose of rotary rigid body spacecraft based on monocular vision as recited in claim 1, wherein said step S6 is to convert all central coordinates (x) into _i ,y _i ,z _i ) Fitting i ∈ {1,2.. n } into a straight line, and calculating a unit direction vector where the straight line is located specifically includes:

the simplified form of the line l is:

wherein a, b, c and d are parameters of a straight line l, and the matrix form converted from (15) is as follows:

all center coordinates

Satisfies the following conditions:

thus:

the direction vector of the straight line l is obtained as N ═ a, c,1, and N is converted into a unit direction vector

8. The method for estimating pose of a rotating rigid body spacecraft based on monocular vision as claimed in claim 1, wherein said step S7, according to the characteristic that all ellipses have their centers in a straight line, and the straight line is the normal of the plane where the ellipses are located, uniquely determines the normal vector and the center coordinates of the plane where the ellipses are located, and calculates the rotation matrix of the plane where the reference ellipse is located with respect to the tracked spacecraft by using the plane where the first ellipse is located as the reference, and the rotation matrix is used as the estimate of pose of the rotating rigid body spacecraft, specifically comprising:

s71: calculating the vector n in the direction of the normal line l of the ellipse and the two normal vectors of the plane of the reference ellipse obtained by S5

Angle therebetween

S72: uniquely determining a normal vector of a plane where a reference ellipse is located and a corresponding central coordinate thereof according to the following principle;

wherein ε < 5 ° represents an angle threshold;

s73: calculating a displacement vector t of the target spacecraft relative to the tracking spacecraft according to the center coordinate of the plane where the uniquely determined reference ellipse is located:

t＝(O _x ,O _y ,O _z ) ^T (21)

wherein (O) _x ,O _y ,O _z ) The center coordinates of the plane where the uniquely determined reference ellipse is located are represented;

the normal vector of the plane of the uniquely determined reference ellipse

As Z _D Axial direction vector

Calculating a vector perpendicular thereto as X _D Axial direction vector

Thereby calculating a rotation matrix R of the target spacecraft relative to the tracking spacecraft:

wherein,

a normal vector representing a plane in which the uniquely determined reference ellipse is located;

the normal vector of the plane of the uniquely determined reference ellipse

As Z _D Axial direction vector

Calculating a vector perpendicular thereto as X _D Axial direction vector

Thereby calculating a rotation matrix R of the target spacecraft relative to the tracking spacecraft:

wherein,

a normal vector representing a plane in which the uniquely determined reference ellipse is located;

in order to facilitate the analysis of the error determined by the relative attitude, the rotation matrix R is written into the form of Euler angle, and the target spacecraft is made to rotate around X _w The angle of rotation of the shaft being pitch phi, about Y _w The angle of rotation of the shaft being yaw angle

Around Z _w The angle of the axis rotation is a roll angle gamma, and the rotation matrix R is expressed in the form of Euler angleBy taking X-rays _w 、Y _w 、Z _w The rotation sequence of (a) is expressed as:

from equation (23), the euler angle can be calculated, expressed as:

wherein R is _ij (i, j e (1,2,3)) represents the i-th row and j-th column element of the rotation matrix R.

Images