阅读说明：本技术 一种基于线特征的无人机惯性/视觉着陆导航方法 (Unmanned aerial vehicle inertia/vision landing navigation method based on line characteristics ) 是由 尚克军 扈光锋 王大元 裴新凯 段昊雨 明丽 庄广琛 刘崇亮 王海军 焦浩 李茜 于 2021-08-13 设计创作，主要内容包括：本发明公开一种基于线特征的无人机惯性/视觉着陆导航方法,首先对机场跑道进行图像采集,对跑道边线进行实时特征提取,获取边线及中线的直线方程；通过提前装订的机场跑道宽度与相机内参矩阵,计算边线方程的Plücker坐标；由两条等距平行线计算无穷远处消隐点及消隐线的方程,通过联立方程组解算无人机实时的世界坐标系与相机坐标系之间的姿态转移矩阵,并进行姿态与侧向、垂向位置的求解；将视觉着陆系统解算出的位置信息作为观测量,与惯性导航输出的导航信息构建卡尔曼滤波器进行融合,实现连续自主的导航定位功能。本发明解决了惯性导航误差随时间累积发散与视觉导航解算结果噪声较大的问题。(The invention discloses an unmanned aerial vehicle inertia/vision landing navigation method based on line characteristics, which comprises the steps of firstly, carrying out image acquisition on an airport runway, carrying out real-time characteristic extraction on the sideline of the runway, and obtaining a linear equation of the sideline and the central line; calculating the Pl ü cker coordinate of a sideline equation through the width of the airport runway bound in advance and the camera internal reference matrix; calculating equations of blanking points and blanking lines at infinity by two equidistant parallel lines, solving an attitude transfer matrix between a real-time world coordinate system and a camera coordinate system of the unmanned aerial vehicle by a simultaneous equation set, and solving the attitude, the lateral position and the vertical position; and taking the position information solved by the visual landing system as observed quantity, and constructing a Kalman filter with navigation information output by inertial navigation to fuse, thereby realizing the continuous and autonomous navigation positioning function. The invention solves the problems of accumulated divergence of inertial navigation errors along with time and higher noise of a visual navigation resolving result.)

1. An unmanned aerial vehicle inertia/vision landing navigation method based on line features is characterized by comprising the following steps:

(1) airport runway data collection

Establishing an airport coordinate system, a visual coordinate system, a world coordinate system, a camera coordinate system and an image coordinate system; carrying out image acquisition on the airport runway, and carrying out real-time feature extraction on the side line of the runway to obtain a linear equation of the side line and the central line;

(2) pl ü cker coordinate representation

Calculating the Pl ü cker coordinate of a sideline equation through the width of the airport runway bound in advance and the camera internal reference matrix;

(3) pose resolution for vision measurement

After obtaining the Pl ü cker coordinates of the runway sideline, calculating equations of a blanking point and a blanking line at infinity by two equidistant parallel lines, and calculating an attitude transfer matrix between a real-time world coordinate system and a camera coordinate system of the unmanned aerial vehicle by a simultaneous equation setSolving the posture, the lateral position and the vertical position;

(4) integrated navigation based on inertial/visual fusion

And taking the position information solved by the visual landing system as observed quantity, and constructing a Kalman filter with navigation information output by inertial navigation to fuse, thereby realizing the continuous and autonomous navigation positioning function.

2. The unmanned aerial vehicle inertial/visual landing navigation method based on line features as claimed in claim 1, wherein the airport coordinate system, the visual coordinate system, the world coordinate system, the camera coordinate system, the image coordinate system are established in step (1), including;

an airport coordinate system marked as a system; the intersection point of the starting line of the landing end of the runway and the central line of the runway is taken as an origin o_a(ii) a The shaft is along the central line of the runway, and the forward direction is positive; y is_aThe axis is vertical to the plane of the runway and is positive upwards; z is a radical of_aThe axis is superposed with the starting line of the runway, and the right direction is positive; o_ax_ay_az_aForming a right-hand coordinate system; (x) coordinates of a point in the airport coordinate system_a,y_a,z_a) Represents;

a visual coordinate system marked as a v system; using the image space principal point of the optical system as the origin o_v；x_vThe axis is parallel to the optical axis, and the forward direction is positive; y is_vThe axis is parallel to the transverse axis of the imaging plane coordinate system and is positive upwards; z is a radical of_vAxis and x_vAxis and y_vThe axes form a right-hand coordinate system, and the right direction is positive;

a world coordinate system marked as a w system; the intersection point of the initial line of the runway landing end aiming point and the centerline of the runway is taken as the origin o_w；x_wThe axis is superposed with the starting line of the runway, and the right direction is positive; y is_wThe axis is vertical to the plane of the runway and is positive downwards; z is a radical of_wThe shaft is along the central line of the runway, and the forward direction is positive; o_wx_wy_wz_wForming a right-hand coordinate system; the coordinate of a certain point in the world coordinate system is (x)_w,y_w,z_w) Represents;

a camera coordinate system, denoted as system c; using the image space principal point of the optical system as the origin o_c(ii) a When viewed directly against the optical system, x_cThe axis is parallel to the horizontal axis of the imaging plane coordinate system, and the left direction is positive; y is_cThe axis is parallel to the vertical axis of the imaging plane coordinate system and is positive downwards; z is a radical of_cThe axis pointing towards the viewer and being parallel to x_cAxis and y_cThe axes form a right-hand coordinate system;

an image coordinate system marked as an i system; establishing an image coordinate system in a plane where a photosurface of the camera is located, taking the upper left corner of the image as an origin and taking the image as the x of the image coordinate system to the right along the horizontal direction of the image_iAxis, downwards in the vertical direction of the image, being y of the image coordinate system_iAxis, the unit of the image coordinate system is a pixel.

3. The unmanned aerial vehicle inertial/visual landing navigation method based on line features as claimed in claim 2, wherein the representation of Pl ü cker coordinates specifically includes:

the equation of a straight line in the image space image coordinate system can be described as:

ax_i+by_i+c＝0

thus, a straight line can be represented by a three-dimensional vector:

l＝[a,b,c]^T

and three-dimensional coordinates of two points A, B in the object space world coordinate system are respectivelyTheir homogeneous coordinates are:the line passing through these two points can be represented by a 4 x 4 antisymmetric homogeneous matrix L, called the Pl ü cker matrix:

L＝AB^T-BA^T

in addition, the straight line L may use its direction vectorExpressed with a moment m, called Pl ü cker coordinates, noted:

wherein the content of the first and second substances,is the direction vector of a straight line, and the moment m is the normal vector of the straight line and the plane defined by the origin, i.e.

The relationship between the Plu cker matrix and the Plu cker coordinates thus obtained is:

under the action of the camera mapping T, a straight line L defined by a Pl ü cker matrix represents an image L of a corresponding straight line in an image coordinate system:

wherein K is a camera internal reference matrix:

is a pose transfer matrix from the world coordinate system to the camera coordinate system,a position vector of an origin of a camera coordinate system in a world coordinate system; s²Only the common coefficient of each parameter in the line, so [ l ]]_×Can be simplified as follows:

4. the method for unmanned aerial vehicle inertial/visual landing navigation based on line features of claim 3, wherein the equation for calculating the vanishing point and the vanishing line at infinity in step (3) comprises:

in the landing process of the unmanned aerial vehicle, the visual landing system extracts runway line features, wherein the left and right sidelines and the central line of the runway are a group of parallel lines which can be used for enteringCalculating the coordinate of a blanking point at infinite distance and a blanking line equation, and setting three equidistant parallel lines on a runway in an object space as L_0w、L_1w、L_2wWhich is imaged in the image plane as_0i、l_1i、l_2iThen the image space blanking point coordinates can be solved by the following relationship:

the vanishing line equation is:

l_∞i＝[(l_0i×l_2i)^T(l_1i×l_2i)]l_1i+2[(l_0i×l_1i)^T(l_2i×l_1i)]l_2i

assume a four-dimensional homogeneous coordinate of a point A in object spaceThen cross point a and the direction isCan be expressed as:

when the parameter lambda is changed from 0 to ∞, the point A is changed from a finite point to an infinitely distant point, and the coordinates of the point in a world coordinate system are as follows:

obtaining the relation between a blanking point and an unmanned aerial vehicle attitude transfer matrix according to an image conjugate equation as follows:

further finishing the equation to obtain:

suppose an image space blanking line l_∞iThe last point is x, and the back projection of the last point in the object space is a directionA straight line of (a); from point x on a straight line, we can get:

x^Tl_∞i＝0

by usingNormal vector n to plane_πOrthogonality yields:

using directions in object space ofThe straight line of (a) is projected as point x of the image space, and then:

transposing the above equation becomes:

in conjunction with the foregoing, the image space blanking line equation can be derived:

the attitude transfer matrix between the real-time world coordinate system and the camera coordinate system of the unmanned aerial vehicle is solved and calculated through a simultaneous equation setAnd the solving of the posture, the lateral position and the vertical position specifically comprises the following steps:

the equations obtained by combining the above equations are as follows:

in the formula (I), the compound is shown in the specification,for the attitude transition matrix:

in the case of an airport runway scenario,is a unit direction vector of the center line of the object space runway,n_πis the unit normal vector of the object space runway plane, n_π＝[0,1,0]^TThe same principle can be obtained

Let K^-1p_∞＝[g₁,g₂,g₃]^T,K^Tl_∞＝[h₁,h₂,h₃]^T,(K^-1p_∞)×(K^Tl_∞)＝[e₁,e₂,e₃]^TSimultaneous attitude transfer matrixFor an antisymmetric array, the sum of the squares of each row and column element is 1, which can be solved by the following equation:

from this three attitude angles are solved:

the relative position is calculated using the line equation:

wherein:

substituting the runway edge equation into the above equation:

straight line L of the same reason₂Determining:

solving for alpha from the above two equations₀、α₂：

Finally solving the vertical and lateral positions t of the unmanned aerial vehicle under the world coordinate system_y、t_x：

5. The unmanned aerial vehicle inertial/visual landing navigation method based on line features of claim 4, wherein in the step (4), the Kalman filtering model continuous state equation of the inertial/visual integrated navigation system is as follows:

wherein F (t) is a continuous state equation state transition matrix at time t,a random noise vector of the system at the time t;

the filtering state quantities are respectively a speed error, a dimensionality error, an altitude error, a longitude error, a north east misalignment angle error, a gyro drift in an XYZ direction of a carrier system and a zero position of an XYZ direction accelerometer of the carrier system;

system state transition matrix

Wherein:

the observation equation is defined as follows:

an observation noise array;

the observed quantity of the integrated navigation system is the difference value between the vertical lateral position output by the inertial navigation of the airport coordinate system and the navigation result of the visual landing system:

H(t)＝[0_3×3 M_3×3 0_3×9]

wherein the content of the first and second substances,

Technical Field

The invention belongs to the technical field of navigation, and particularly relates to a landing navigation method for an unmanned aerial vehicle.

Background

The line characteristics applied in the pose resolving process of the unmanned aerial vehicle visual landing are two sidelines and a central line of an airport runway and a far disappearing line. Different from point features, line features are not easily affected by factors such as illumination, flight distance and height, and have better robustness, but because the features of the hidden lines are not obvious enough and are difficult to identify through a feature extraction mode, a new scheme is needed to obtain the hidden line equation in the image plane. Meanwhile, the situation that the pose calculation result of the visual navigation mode is not smooth enough and jumps along with the rapid maneuver of the unmanned aerial vehicle needs to be considered, and a filtering method is needed to fuse the inertial navigation result and the visual navigation result.

Disclosure of Invention

The invention provides an unmanned aerial vehicle inertial/visual landing navigation method based on line characteristics, which solves the problems of inertial navigation error accumulation and divergence along with time and larger noise of a visual navigation resolving result.

An unmanned aerial vehicle inertia/vision landing navigation method based on line characteristics comprises the following steps:

(1) airport runway data collection

Establishing an airport coordinate system, a visual coordinate system, a world coordinate system, a camera coordinate system and an image coordinate system; carrying out image acquisition on the airport runway, and carrying out real-time feature extraction on the side line of the runway to obtain a linear equation of the side line and the central line;

(2) pl ü cker coordinate representation

Calculating the Pl ü cker coordinate of a sideline equation through the width of the airport runway bound in advance and the camera internal reference matrix;

(3) pose resolution for vision measurement

After obtaining the Pl ü cker coordinates of the runway sideline, calculating equations of a blanking point and a blanking line at infinity by two equidistant parallel lines, and calculating an attitude transfer matrix C between a real-time world coordinate system and a camera coordinate system of the unmanned aerial vehicle by a simultaneous equation set_wc, solving the posture, the lateral position and the vertical position;

(4) integrated navigation based on inertial/visual fusion

And taking the position information solved by the visual landing system as observed quantity, and constructing a Kalman filter with navigation information output by inertial navigation to fuse, thereby realizing the continuous and autonomous navigation positioning function.

Further, establishing an airport coordinate system, a visual coordinate system, a world coordinate system, a camera coordinate system and an image coordinate system in the step (1), wherein the steps comprise;

an airport coordinate system marked as a system; the intersection point of the starting line of the landing end of the runway and the central line of the runway is taken as an origin o_a(ii) a The shaft is along the central line of the runway, and the forward direction is positive; y is_aThe axis is vertical to the plane of the runway and is positive upwards; z is a radical of_aThe axis is superposed with the starting line of the runway, and the right direction is positive; o_ax_ay_az_aForming a right-hand coordinate system; (x) coordinates of a point in the airport coordinate system_a,y_a,z_a) Represents;

a visual coordinate system marked as a v system; using the image space principal point of the optical system as the origin o_v；x_vThe axis is parallel to the optical axis, and the forward direction is positive; y is_vThe axis is parallel to the transverse axis of the imaging plane coordinate system and is positive upwards; z is a radical of_vAxis and x_vAxis and y_vThe axes form a right-hand coordinate system, and the right direction is positive;

a world coordinate system marked as a w system; the intersection point of the initial line of the runway landing end aiming point and the centerline of the runway is taken as the origin o_w；x_wThe axis is superposed with the starting line of the runway, and the right direction is positive; y is_wThe axis is vertical to the plane of the runway and is positive downwards; z is a radical of_wThe shaft is along the central line of the runway, and the forward direction is positive; o_wx_wy_wz_wForming a right-hand coordinate system; the coordinate of a certain point in the world coordinate system is (x)_w,y_w,z_w) Represents;

a camera coordinate system, denoted as system c; using the image space principal point of the optical system as the origin o_c(ii) a When viewed directly against the optical system, x_cThe axis is parallel to the horizontal axis of the imaging plane coordinate system, and the left direction is positive; y is_cThe axis is parallel to the vertical axis of the imaging plane coordinate system and is positive downwards; z is a radical of_cThe axis pointing towards the viewer and being parallel to x_cAxis and y_cThe axes form a right-hand coordinate system;

an image coordinate system marked as an i system; establishing an image coordinate system in a plane where a photosurface of the camera is located, taking the upper left corner of the image as an origin and taking the image as the x of the image coordinate system to the right along the horizontal direction of the image_iAxis, downwards in the vertical direction of the image, being y of the image coordinate system_iAxis, the unit of the image coordinate system is a pixel.

Further, the Pl ü cker coordinate representation specifically includes:

the equation of a straight line in the image space image coordinate system can be described as:

ax_i+by_i+c＝0

thus, a straight line can be represented by a three-dimensional vector:

l＝[a,b,c]^T

and three-dimensional coordinates of two points A, B in the object space world coordinate system are respectivelyTheir homogeneous coordinates are:the line passing through these two points can be represented by a 4 x 4 antisymmetric homogeneous matrix L, called the Pl ü cker matrix:

L＝AB^T-BA^T

in addition, the straight line L may use its direction vectorExpressed with a moment m, called Pl ü cker coordinates, noted:

wherein the content of the first and second substances,is the direction vector of a straight line, and the moment m is the normal vector of the straight line and the plane defined by the origin, i.e.

The relationship between the Plu cker matrix and the Plu cker coordinates thus obtained is:

under the action of the camera mapping T, a straight line L defined by a Pl ü cker matrix represents an image L of a corresponding straight line in an image coordinate system:

wherein K is a camera internal reference matrix:

is a pose transfer matrix from the world coordinate system to the camera coordinate system,a position vector of an origin of a camera coordinate system in a world coordinate system; s²Only the common coefficient of each parameter in the line, so [ l ]]_×Simple and easy to useThe method comprises the following steps:

further, the equation for calculating the blank points and the blank lines at infinity in step (3) comprises:

in the landing process of the unmanned aerial vehicle, the visual landing system extracts runway line features, wherein the left and right sidelines and the central line of the runway are a group of parallel lines which can be used for calculating the coordinate of a blank point at an infinite distance and a blank line equation, and three equidistant parallel lines on the runway in an object space are set as L_0w、L_1w、L_2wWhich is imaged in the image plane as_0i、l_1i、l_2iThen the image space blanking point coordinates can be solved by the following relationship:

the vanishing line equation is:

l_∞i＝[(l_0i×l_2i)^T(l_1i×l_2i)]l_1i+2[(l_0i×l_1i)^T(l_2i×l_1i)]l_2i

assume a four-dimensional homogeneous coordinate of a point A in object spaceThen cross point a and the direction isCan be expressed as:

when the parameter lambda is changed from 0 to ∞, the point A is changed from a finite point to an infinitely distant point, and the coordinates of the point in a world coordinate system are as follows:

obtaining the relation between a blanking point and an unmanned aerial vehicle attitude transfer matrix according to an image conjugate equation as follows:

further finishing the equation to obtain:

suppose an image space blanking line l_∞iThe last point is x, and the back projection of the last point in the object space is a directionA straight line of (a); from point x on a straight line, we can get:

x^Tl_∞i＝0

by usingNormal vector n to plane_πOrthogonality yields:

using directions in object space ofThe straight line of (a) is projected as point x of the image space, and then:

transposing the above equation becomes:

in conjunction with the foregoing, the image space blanking line equation can be derived:

the attitude transfer matrix between the real-time world coordinate system and the camera coordinate system of the unmanned aerial vehicle is solved and calculated through a simultaneous equation setAnd the solving of the posture, the lateral position and the vertical position specifically comprises the following steps:

the equations obtained by combining the above equations are as follows:

in the formula (I), the compound is shown in the specification,for the attitude transition matrix:

in the case of an airport runway scenario,is a unit direction vector of the center line of the object space runway,n_πis the unit normal vector of the object space runway plane, n_π＝[0,1,0]^TThe same principle can be obtained

Let K^-1p_∞＝[g₁,g₂,g₃]^T,K^Tl_∞＝[h₁,h₂,h₃]^T,(K^-1p_∞)×(K^Tl_∞)＝[e₁,e₂,e₃]^TWhile attitude transition matrix C_wc is an antisymmetric array, the sum of squares of each row and column element is 1, and the formula can be solved as follows:

from this three attitude angles are solved:

the relative position is calculated using the line equation:

wherein:

substituting the runway edge equation into the above equation:

straight line L of the same reason₂Determining:

solving for alpha from the above two equations₀、α₂：

Finally solving the vertical and lateral positions t of the unmanned aerial vehicle under the world coordinate system_y、t_x：

Further, in step (4), the continuous state equation of the kalman filtering model of the integrated inertial/visual navigation system is as follows:

wherein F (t) is a continuous state equation state transition matrix at time t,a random noise vector of the system at the time t;

the filtering state quantities are respectively a speed error, a dimensionality error, an altitude error, a longitude error, a north east misalignment angle error, a gyro drift in an XYZ direction of a carrier system and a zero position of an XYZ direction accelerometer of the carrier system;

system state transition matrix

Wherein:

the observation equation is defined as follows:

an observation noise array;

the observed quantity of the integrated navigation system is the difference value between the vertical lateral position output by the inertial navigation of the airport coordinate system and the navigation result of the visual landing system:

H(t)＝[0_3×3 M_3×3 0_3×9]

wherein the content of the first and second substances,

the invention provides an infinite element and a Plu cker coordinate representation method thereof, and solves the problem that the intersection point coordinate of parallel lines in an object space on an image plane cannot be solved. And secondly, solving a hidden line elimination equation at infinity by a group of equidistant parallel lines of the left and right side lines and the central line of the runway, thereby solving the problem that hidden lines cannot be identified by a feature extraction method. And finally, the pose of the unmanned aerial vehicle is solved and fused with inertia through a hidden line elimination equation, so that the problems of accumulated divergence of inertial navigation errors along with time and higher noise of a visual navigation resolving result are solved.

Drawings

FIG. 1 is a schematic view of a coordinate system;

FIG. 2 is a schematic diagram of a set of parallel lines intersecting at a blanking point;

FIG. 3 is a schematic drawing showing the meaning of the Pl ü cker coordinates in a straight line.

Detailed Description

The present invention will be described in further detail with reference to the accompanying drawings.

Aiming at the problem of autonomous landing navigation of an unmanned aerial vehicle under the satellite rejection condition, the invention develops the research of an inertial/visual landing navigation method based on line characteristics, firstly provides an infinite element and a Plucker coordinate representation method thereof, and solves the problem that the intersection point coordinate of parallel lines in an object space on an image plane cannot be solved. And secondly, solving a hidden line elimination equation at infinity by a group of equidistant parallel lines of the left and right side lines and the central line of the runway, thereby solving the problem that hidden lines cannot be identified by a feature extraction method. And finally, the pose of the unmanned aerial vehicle is solved and fused with inertia through a hidden line elimination equation, so that the problems of accumulated divergence of inertial navigation errors along with time and higher noise of a visual navigation resolving result are solved.

1. Infinite element and Pl ü cker representation

(1) Definition of coordinate system

As shown in fig. 1, an airport coordinate system, a visual coordinate system, a world coordinate system, a camera coordinate system, and an image coordinate system are established.

Wherein the airport coordinate system (a system): the intersection point of the starting line of the landing end of the runway and the central line of the runway is taken as an origin o_a(ii) a The shaft is along the central line of the runway, and the forward direction is positive; y is_aThe axis is vertical to the plane of the runway and is positive upwards; z is a radical of_aThe axis is superposed with the starting line of the runway, and the right direction is positive; o_ax_ay_az_aForming a right-hand coordinate system; (x) coordinates of a point in the airport coordinate system_a,y_a,z_a) And (4) showing.

Visual coordinate system (v series): a landing visual navigation system coordinate system, which is called a visual coordinate system for short; using the image space principal point of the optical system as the origin o_v；x_vThe axis is parallel to the optical axis, and the forward direction is positive; y is_vThe axis is parallel to the transverse axis of the imaging plane coordinate system and is positive upwards; z is a radical of_vAxis and x_vAxis and y_vThe axes form a right-hand coordinate system, the right direction being positive.

World coordinate system (w system): the intersection point of the initial line of the runway landing end aiming point and the centerline of the runway is taken as the origin o_w；x_wThe axis is superposed with the starting line of the runway, and the right direction is positive; y is_wThe axis is vertical to the plane of the runway and is positive downwards; z is a radical of_wThe shaft is along the central line of the runway, and the forward direction is positive; o_wx_wy_wz_wForming a right-hand coordinate system; the coordinate of a certain point in the world coordinate system is (x)_w,y_w,z_w) And (4) showing.

Camera coordinate system (series c): using the image space principal point of the optical system as the origin o_c(ii) a When viewed directly against the optical system, x_cThe axis is parallel to the horizontal axis of the imaging plane coordinate system, and the left direction is positive; y is_cThe axis is parallel to the vertical axis of the imaging plane coordinate system and is positive downwards; z is a radical of_cThe axis pointing towards the viewer and being parallel to x_cAxis and y_cShaft structureRight hand coordinate system.

Image coordinate system (i system): establishing an image coordinate system in a plane where a photosurface of the camera is positioned, wherein the image coordinate system is a two-dimensional plane coordinate system, and the image coordinate system x is taken as the right side along the horizontal direction of an image by taking the upper left corner of the image as an origin_iAxis, downwards in the vertical direction of the image, being y of the image coordinate system_iAxis, the unit of the image coordinate system is a pixel.

(2) Elements of infinity

In object space, two parallel lines never intersect, a projective space is constructed by introducing an infinite element on the basis of Euclidean space, and a group of parallel lines in a plane intersect at a unique Point at infinite distance, which is called a blanking Point (vanising Point). As shown in fig. 2. The position of this point on the image plane is only dependent on the pose of the camera and not on the position of the camera.

The blanking points represent the directions of corresponding parallel lines, infinite points of non-parallel lines are different, and all infinite points on a plane form a straight Line, namely a blanking Line (vanising Line). The vanishing line is the only intersection line of a group of parallel planes in space at infinite distance.

(3) Pl ü cker notation

The equation of a straight line in the image space image coordinate system can be described as:

ax_i+by_i+c＝0

thus, a straight line can be represented by a three-dimensional vector:

l＝[a,b,c]^T

and three-dimensional coordinates of two points A, B in the object space world coordinate system are respectively(3 × 1 matrix), then their homogeneous coordinates are:the line passing through these two points can be represented by a 4 x 4 antisymmetric homogeneous matrix L, known as the Pl ü cker matrix.

L＝AB^T-BA^T

In addition, the straight line L may use its direction vectorExpressed with a moment m, called Pl ü cker coordinates, noted:

wherein the content of the first and second substances,is the direction vector of a straight line, and the moment m (the area that can characterize Δ ABC or the distance O from the straight line L) is the normal vector of the straight line and the origin-defining plane, i.e. the(as shown in FIG. 3):

the relationship between the Plu cker matrix and the Plu cker coordinates thus obtained is:

under the action of the camera mapping T, a straight line L defined by a Pl ü cker matrix represents an image L of a corresponding straight line in an image coordinate system:

wherein K is a camera internal reference matrix:

is a pose transfer matrix from the world coordinate system to the camera coordinate system,is the position vector of the origin of the camera coordinate system in the world coordinate system. s²Only the common coefficient of each parameter in the line, so [ l ]]_×Can be simplified as follows:

2. pose resolution for vision measurement

(1) Imaging equation of blanking point and blanking line

In the landing process of the unmanned aerial vehicle, the visual landing system extracts runway line features, wherein the left and right sidelines and the central line of the runway are a group of parallel lines which can be used for calculating the coordinate of a blank point at an infinite distance and a blank line equation, and three equidistant parallel lines on the runway in an object space are set as L_0w、L_1w、L_2wWhich is imaged in the image plane as_0i、l_1i、l_2iThen the image space blanking point coordinates can be solved by the following relationship:

the vanishing line equation is:

l_∞i＝[(l_0i×l_2i)^T(l_1i×l_2i)]l_1i+2[(l_0i×l_1i)^T(l_2i×l_1i)]l_2i

assume a four-dimensional homogeneous coordinate of a point A in object spaceThen cross point a and the direction is

(three-dimensional unit column vector) which can be expressed as:

when the parameter lambda is changed from 0 to ∞, the point A is changed from a finite point to an infinitely distant point, and the coordinates of the point in a world coordinate system are as follows:

obtaining the relation between a blanking point and an unmanned aerial vehicle attitude transfer matrix according to an image conjugate equation as follows:

further finishing the equation to obtain:

suppose an image space blanking line l_∞iThe last point is x, and the back projection of the last point in the object space is a directionIs measured. From point x on a straight line, we can get:

x^Tl_∞i＝0

by usingNormal vector n to plane_πOrthogonality yields:

using directions in object space ofProjected as points in image spacex, obtaining:

transposing the above equation becomes:

in conjunction with the foregoing, the image space blanking line equation can be derived:

(2) pose resolving of unmanned aerial vehicle

The equations given in the previous equations can be found as follows:

in the formula (I), the compound is shown in the specification,for the attitude transition matrix:

in the case of an airport runway scenario,is a unit direction vector of the center line of the object space runway,n_πis the unit normal vector of the object space runway plane, n_π＝[0,1,0]^TThe same principle can be obtained

Let K^-1p_∞＝[g₁,g₂,g₃]^T,K^Tl_∞＝[h₁,h₂,h₃]^T,(K^-1p_∞)×(K^Tl_∞)＝[e₁,e₂,e₃]^TWhile attitude transition matrix C_wc is an antisymmetric array, the sum of squares of each row and column element is 1, and the formula can be solved as follows:

from this three attitude angles are solved:

the relative position is calculated using the line equation:

wherein:

substituting the runway edge equation into the above equation:

straight line L of the same reason₂Determining:

solving for alpha from the above two equations₀、α₂：

Finally solving the vertical and lateral positions t of the unmanned aerial vehicle under the world coordinate system_y、t_x：

3. Inertial/visual fusion method

The Kalman filtering model continuous state equation of the inertial/visual integrated navigation system is as follows:

wherein F (t) is a continuous state equation state transition matrix at time t,is the random noise vector of the system at the time t.

The filtering state quantities are respectively velocity error (unit: m/s) of north heaven and east, dimensionality error (unit: rad), altitude error (unit: m), longitude error (unit: rad), east misalignment angle error (unit: rad) of north heaven, gyroscope drift (unit: rad/s) of XYZ direction of carrier system, and zero position (unit: m/s) of XYZ direction accelerometer of carrier system²)。

System state transition matrix

Wherein:

the observation equation is defined as follows:

to observe the noise array.

The observed quantity of the integrated navigation system is the difference value between the vertical and lateral position output by the inertial navigation of the airport coordinate system and the navigation result of the visual landing system, and comprises the following steps:

H(t)＝[0_3×3 M_3×3 0_3×9]

wherein the content of the first and second substances,

in conclusion, an inertial/visual navigation method for the unmanned aerial vehicle to land by utilizing three equidistant parallel lines of the left and right side lines and the central line of the runway is provided.

The unmanned aerial vehicle can land autonomously through the following 4 processes:

(1) collecting airport runway data:

the method comprises the steps of carrying out image acquisition on an airport runway through a front-view infrared vision navigation system installed on an unmanned aerial vehicle head, carrying out real-time feature extraction on the sideline of the runway, and obtaining a linear equation of the sideline and a central line.

(2) Pl ü cker coordinates:

and calculating the Pl ü cker coordinate of the sideline equation according to the above-mentioned deduction formula through the width of the airport runway bound in advance and the camera internal reference matrix.

(3) And (3) resolving the vision measurement pose:

after obtaining the Pl ü cker coordinates of the runway sideline, calculating equations of a blanking point and a blanking line at infinity by two equidistant parallel lines, and calculating an attitude transfer matrix between a real-time world coordinate system and a camera coordinate system of the unmanned aerial vehicle by a simultaneous equation setAnd solving the posture, the lateral position and the vertical position.

(4) Integrated navigation based on inertial/visual fusion:

and taking the position information solved by the visual landing system as observed quantity, and constructing a Kalman filter with navigation information output by inertial navigation to fuse, thereby realizing the continuous and autonomous navigation positioning function.