Inertial vision integrated navigation method and device based on multi-domain conversion

Abstract

The invention provides an inertial vision integrated navigation method and device based on multi-domain conversion. The method comprises the following steps: S1, acquiring database information of an inertial vision integrated navigation system; s2, initializing a system error state in the navigation domain, and performing time updating on the system state in the navigation domain according to a time updating principle of the system in the navigation domain; s3, converting the navigation domain state and the uncertainty matrix into a runway domain state and an uncertainty matrix; s4, if the camera collects the visual image at the current moment, performing an intelligent detection algorithm on the visual image at the current moment to obtain visual measurement on the measurement domain; s2, according to the visual measurement, updating a system state # imgabs0 # and an uncertainty matrix # imgabs1 # thereof in a runway domain, and if the camera does not acquire a visual image at the current moment, returning to the step S2; and S5, converting the runway domain state # imgabs2 # and the uncertainty matrix # imgabs3 # into a navigation domain state # imgabs4 # and the uncertainty matrix # imgabs5 # so as to realize inertial vision integrated navigation.

Description

Technical Field

[0001] The present invention relates to the technical field of aviation, and in particular to an inertial vision integrated navigation method and device based on multi-domain conversion. Background Art

[0002] During the runway landing of a large aircraft, the pilot needs the navigation system to provide the current position, attitude and other information to ensure that the aircraft can land safely on the target runway. This navigation technology designs a variety of navigation equipment and technologies. In the field of modern aviation, commonly used navigation equipment includes the Global Positioning System (GPS), Inertial Navigation System (INS), Microwave Landing System (MLS), Instrument Landing System (ILS) and GBAS landing system. The current mainstream landing navigation system is ILS, which is the most widely used precision approach and landing guidance system, relying on radio signals to guide the aircraft's approach.

[0003] ILS systems are divided into three categories, and only the most expensive Category III system can achieve the ability to land without relying on pilots. However, existing ILS equipment has the problem of relying on ground equipment and being expensive.

[0004] For some airports that lack high-precision ground navigation facilities, how to establish a reasonably priced navigation method that meets the requirements for autonomous landing is an urgent need for pilots around the world. Summary of the invention

[0005] The present invention provides an inertial vision combined navigation method and device based on multi-domain conversion, which can solve the problem that the existing navigation equipment is dependent on ground equipment and is expensive.

[0006] The technical solution of the present invention:

[0007] In a first aspect, the present application provides an inertial vision integrated navigation method based on multi-domain conversion, the method comprising:

[0008] S1. Obtaining database information of the inertial vision integrated navigation system;

[0009] S2, initializing the system error state in the navigation domain, and updating the system state in the navigation domain according to the time update principle of the system in the navigation domain;

[0010] S3, converting the navigation domain state and uncertainty matrix into the runway domain state and uncertainty matrix;

[0011] S4: If the camera captures the visual image at the current moment, the visual measurement in the measurement domain is obtained by using the intelligent detection algorithm for the visual image at the current moment; based on the visual measurement, the system status in the runway domain is updated. and its uncertainty matrix If the camera does not capture the visual image at the current moment, return to step S2;

[0012] S5, the runway domain status and its uncertainty matrix Transition to navigation domain state and its uncertainty matrix Thereby achieving inertial and visual combined navigation.

[0013] Specifically, S2 includes:

[0014] S21. Determine the navigation domain of the inertial vision integrated navigation system according to the WGS 84 coordinate system, the local navigation system, the aircraft system and the camera system;

[0015] S22, determining the navigation domain state x:

[0016] x=[δp n ,δv n ,δγ n , b g , b a ] T

[0017] In the above formula, δp n represents the position error in the local navigation system, δv n represents the velocity error in the local navigation system, δγ n Represents the misalignment angle error in the local navigation system;

[0018] S23, initializing the navigation domain state x0;

[0019] S24: According to the time update principle of the system under the navigation domain, the navigation domain state at the kth moment is updated. k

[0020] Specifically, S3 includes:

[0021] S31. Determine a runway domain according to a target runway coordinate system;

[0022] S32. Determine the runway domain state x′ according to the user's concerns and visual measurement characteristics:

[0023] x′=[δp r ,δγ r ] T

[0024] In the above formula, δγ r represents the position error of the system projection in the runway system, δγ r It represents the angular error of the system projection under the runway system;

[0025] S33, the system position error δp in the navigation domain n , converted to the system position error δp in the runway domain r ;

[0026]

[0027] in, is the rotation matrix from the navigation system to the runway system, δp r represents the position error projection of the system in the r system, δp n represents the position error projection of the system in the n-frame;

[0028] S34, the system angle error δγ in the navigation domain n , converted to the system angle error δγ in the runway domain r ;

[0029] S35, the error uncertainty matrix P in the navigation domain n , converted to the error uncertainty matrix P in the runway domain r .

[0030] Specifically, S34 includes:

[0031] Using the formula The system angle error δγ in the navigation domain n , converted to the system angle error δγ in the runway domain r ;

[0032] Among them, exp((δγ n )^) is the rotation vector δγ n The corresponding rotation matrix, exp((δγ r )^) is the rotation vector δγ r The corresponding rotation matrix is Represents the rotation matrix from n system to r system.

[0033] Specifically, S35 includes:

[0034] S351, Solve P n The eigenvalue d n and the eigenvector v n ;

[0035] S352. Use formula: d r =d n Solving for P r The eigenvalue d r and the eigenvector v r ;

[0036] S353, using formula P r =vr d r (v r )-1, solve the error uncertainty matrix P in the runway domain r .

[0037] Specifically, S4 includes:

[0038] S41 determines a measurement domain according to sensor data of an inertial vision integrated navigation system;

[0039] S42, obtain the full amount of aircraft position and velocity information under the navigation system from the external inertial navigation high frequency, where the inertial navigation solution position is p n ;

[0040] S43, acquiring the image information and the corresponding timestamp obtained by the visual sensor at a lower frequency, and using the intelligent detection algorithm to obtain the visual measurement results θ1, θ2, u, v; wherein θ1, θ2 represent the pixel slopes of the two feature lines, and u, v represent the pixel coordinates of a feature point;

[0041] S44, respectively determine the predicted values ​​of the visual measurement results θ1, θ2, u, and v

[0042] S45. According to the predicted value And visual measurement results visual measurement results θ1, θ2, u, v, using Determine the total filter measurement y;

[0043] S46, solving the filter observation matrix H according to the database information of the inertial vision integrated navigation system and the full amount of posture and velocity information of the aircraft under the navigation system;

[0044] S47 performs status update and obtains the updated runway domain status and its runway domain uncertainty matrix

[0045] Specifically, S46 includes:

[0046] S461, according to the rotation matrix from n system to c system Determine the parameter r 11 , r 12 ,...r 88 ;

[0047]

[0048] S462, according to δγ r , using the formula θa=δγ r Solve for parameters θ and a;

[0049] S463, according to the parameters θ, a, using the formula Determine parameter J;

[0050] S464, based on the database information of the inertial vision integrated navigation system, the full position and speed information of the aircraft under the navigation system, and the actual runway domain position M of the feature point to be observed r , determine the camera position of the feature point to be observed

[0051]

[0052] in, is the rotation matrix from the runway system to the camera system;

[0053] S465, calculate the position p according to the inertial navigation n , the rotation matrix from the navigation system to the camera system State δp n and the camera and the aircraft's boom arm Using the formula Determine the runway coordinates of the current camera position

[0054] S466, according to parameter r 11 , r 12 ,...r 33 , runway width b, determine parameters a1~a4;

[0055] a1=r 11 r 22 r 33 -r 11 r 23 r 32 -r 12 r 21 r 33 +r 12 r 23 r 31 +r 13 r 21 r 32 -r 13 r 22 r 31 ;

[0056] a2=r 11 r 32 br 12 r 31 b;

[0057]

[0058] S467, determine the parameter f according to the camera internal parameter K x 、f y ;

[0059] S468, according to parameters a1~a4, f x 、f y , the runway coordinates of the current camera position Parameter r 11 , r 12 ,...r 33 Determine the parameters h11, h12, h21, h22;

[0060]

[0061] S469, according to parameters h11, h12, h21, h22, f x 、f y , J, camera position of the feature point to be observed The actual runway domain position M of the feature point to be observed r , the rotation matrix from the track system to the camera system Solve the matrix H;

[0062]

[0063] Specifically, step 44 includes:

[0064] The prediction of point features can be expressed as:

[0065]

[0066] The prediction of line features can be expressed as:

[0067] l=(K[RT])L(K[PT]) T

[0068] In the above formula, M c is the position coordinate of the feature point in the camera system, M r is the position coordinate of the feature point in the runway system, [RT] represents the rotation and translation relationship of projecting the point coordinates from the runway system to the camera system, K is the internal parameter, s is the normalization parameter, L represents the Plücker matrix of the three-dimensional line, and l represents the Plücker matrix of the two-dimensional line.

[0069] In a second aspect, the present application provides an inertial vision integrated navigation device based on multi-domain conversion, and the device is used to implement the above-mentioned inertial vision integrated navigation method based on multi-domain conversion.

[0070] Beneficial effects of the invention: The invention aims at the traditional runway autonomous landing navigation technology (ILS) which relies on ground equipment, and provides an inertial vision combined navigation method and device based on multi-domain conversion. It utilizes the feature information and inertial navigation information collected by vision to achieve navigation performance under high-precision requirements such as runway landing of large aircraft. It is a navigation method that supplements ILS\GNSS and can ensure high-precision and reliable navigation and positioning capabilities when relying solely on airborne equipment. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 This is the data flow diagram of the traditional inertial and visual combined navigation method;

[0072] Figure 2 It is a data flow diagram of an inertial vision integrated navigation method based on multi-domain conversion;

[0073] Figure 3 is a schematic diagram of a pixel system;

[0074] Figure 4 This is a schematic diagram of the runway system;

[0075] Figure 5 This is a schematic diagram of the WGS84 system;

[0076] Figure 6 It is a schematic diagram of the machine system;

[0077] Figure 7 Figure 2 is a schematic diagram of the overall filtering algorithm flow. DETAILED DESCRIPTION

[0078] As an emerging navigation technology, visual navigation technology, combined with inertial navigation technology, can use information such as images and internal states to achieve high-precision autonomous navigation capabilities. Thanks to the development of artificial intelligence visual perception technology, when satellite signals are limited or completely denied and ground equipment cannot provide information, inertial visual combined navigation technology can rely on ground features and existing databases to provide navigation information such as the current carrier position.

[0079] In order to meet the above challenges, the present invention proposes an inertial vision integrated navigation method based on multi-domain conversion. The system aims to solve the problems of low accuracy, susceptibility to electromagnetic interference and high price of traditional runway autonomous landing navigation technology (ILS) in the absence of ground auxiliary equipment, and ensure high-precision and reliable navigation and positioning capabilities in complex environments.

[0080] The traditional navigation filter used in this scenario maintains the navigation status of the system in the navigation domain to correct the inertial navigation information. Only the data in the navigation domain is continuous; the runway domain information is available only when there is measurement. For details, see Figure 1. Generally speaking, users are more concerned about the relative position of the current aircraft relative to the runway, and the information in the navigation domain is less intuitive than the information in the runway domain. The algorithm state proposed in this scheme is maintained in the navigation domain and the runway domain, and the data of the two coordinate systems are continuous. Compared with the traditional method, it has the great advantages of meeting the user's concerns, conforming to the visual observation characteristics, providing the possibility for subsequent integrity calculation, and being able to correct the inertial navigation information.

[0081] Combined with Figure 2 , describe the technical solution in detail. After the database is determined, the required point and line features are detected in the image data using artificial intelligence algorithms. At this time, the point and line features are in the measurement domain and are represented by a pixel coordinate system; the inertial navigation and visual information are fused using a tight combination method to obtain runway domain navigation data, and then the conversion relationship between domains is used to obtain navigation domain navigation information. When there is no observation, the navigation domain data is only updated in time and then converted to the runway domain.

[0082] Embodiment 1

[0083] The present application provides an inertial vision integrated navigation method based on multi-domain conversion, comprising the following steps:

[0084] S1. Obtain the database information of the inertial vision integrated navigation system, including various parameters of the inertial navigation, various parameters of the camera, calibration parameters of the camera and the aircraft, calibration parameters of the inertial navigation and the aircraft, latitude and longitude of the target runway entrance, runway direction, runway length and width, flight trajectory and other information.

[0085] Specifically, S1 includes the following steps:

[0086] Obtain inertial navigation parameters. According to the selected inertial navigation signal, check the manual to obtain the zero bias, random walk, zero-position first-order Markov parameters of the inertial navigation gyro and the zero bias, random walk, zero-position first-order Markov parameters of the inertial navigation adder, and the inertial navigation update frequency f ins .

[0087] Camera parameter acquisition. Including camera intrinsic parameter K, camera distortion parameter C, camera sampling frequency f vis .

[0088] Determine the calibration parameters of the camera and the body, including the translation vector from the camera system to the origin of the body system The rotation matrix from the camera system to the camera system Determine the calibration parameters of the camera and inertial navigation, including the translation vector from the camera system to the origin of the inertial system The rotation matrix from the camera system to the inertial system

[0089] Determine the target runway information, including the latitude and longitude of the runway entrance point lon0, lat0, height0, the runway heading angle β, the runway length l and width b. Determine a landing trajectory that eventually lands 400m from the runway entrance. Determine the runway system coordinates M of the observation point feature r .

[0090] S2, initializing the system error state in the navigation domain, and updating the system state in the navigation domain according to the time update principle of the system in the navigation domain.

[0091] Specifically, S2 includes:

[0092] S21. Determine the navigation domain of the inertial vision integrated navigation system according to the WGS 84 coordinate system, the local navigation system, the aircraft system and the camera system.

[0093] It should be noted that the navigation domain is related to the WGS 84 coordinate system, local navigation system, aircraft system, and camera system. WGS-84 is the main satellite navigation system coordinate system currently used. I will not go into details here. For details, see Figure 5 The local navigation system is set to the n system, the three axes coincide with the northeast celestial coordinate system, and the origin is selected to coincide with the runway system; the aircraft system is set to the b system, defined as the front lower right, and the origin is the center of mass of the aircraft, see Figure 6 The camera system is set to the c system, defined as the lower right front, and the origin is the optical center of the camera.

[0094] S22, determining the navigation domain state x:

[0095] x=[δp n ,δv n ,δγ n , b g , b a ] T

[0096] In the above formula, δp n represents the position error in the local navigation system, δv n represents the velocity error in the local navigation system, δγ n Represents the misalignment angle error in the local navigation system.

[0097] From the definition of misalignment angle, we know that b g represents the gyro's zero bias error in frame b, b a Represents the zero bias error of the accelerometer in frame b.

[0098] S23. Initialize the navigation domain state x0.

[0099] Initialize a reasonable navigation domain state based on the inertial navigation parameters in the database information.

[0100] S24, according to the time update principle of the system under the navigation domain, the navigation domain state x at the kth moment k Update the time.

[0101] In the present invention, time update always occurs in the navigation domain system state, and measurement update always occurs in the runway domain state. Time update refers to the update matrix Φ of the high-precision inertial navigation. k , which will not be elaborated here. Based on this, the time update equation of the navigation domain state is obtained:

[0102]

[0103] In the above formula, represents the predicted value of the navigation domain state at time k|1, w k is white noise.

[0104] S3, converts the navigation domain state and uncertainty matrix into the runway domain state and uncertainty matrix.

[0105] Specifically, S3 includes:

[0106] S31. Determine a runway domain according to a target runway coordinate system.

[0107] The runway domain is based on the target runway coordinate system. The feature results obtained from the measurement will be converted to the runway domain to obtain the position of the target carrier relative to the runway system. Generally, the runway is regarded as a horizontal plane rectangle. When obtaining the prior data of the runway, it is necessary to obtain the longitude and latitude coordinates of the runway entrance point (or other deterministic description position), the runway orientation, the length and width of the runway, the horizontal inclination angle of the runway, and other data to determine the position and angle of the runway system in the geocentric coordinate system. The runway system is set to the r system, which is defined as: the center of the runway entrance in the current flight direction is the origin of the runway system, the z direction is perpendicular to the runway plane and points to the center of the earth, the x direction is along the runway and points to the tail end of the runway, and the y direction is perpendicular to the runway direction, following the right-hand rule. See for details. Figure 4 .

[0108] S32. Determine the runway domain state χ′ according to the user's concerns and visual measurement characteristics:

[0109] x′=[δp r ,δγ r ] T

[0110] In the above formula, δp r represents the position error of the system projection in the runway system, δγ r It represents the angular error of the system projection in the runway system.

[0111] S33, the system position error δp in the navigation domainn , converted to the system position error δp in the runway domain r .

[0112]

[0113] in, is the rotation matrix from the navigation system to the runway system, δp r represents the position error projection of the system in the r system, δp n Represents the position error projection of the system in the n system.

[0114] S34, the system angle error δγ in the navigation domain n , converted to the system angle error δγ in the runway domain r .

[0115] Specifically, S34 includes:

[0116] Using the formula The system angle error δγ in the navigation domain n , converted to the system angle error δγ in the runway domain r .

[0117] Among them, exp((δγ n )^) is the rotation vector δγ n The corresponding rotation matrix, exp((δγ r )^) is the rotation vector δγ r The corresponding rotation matrix is Represents the rotation matrix from the n system to the runway coordinate system.

[0118] It needs to be explained that the rotation vector δγ n The corresponding rotation matrix can be expressed as exp((δγ n )^), this is because the rotation vector can be regarded as Lie algebra, and the exponential mapping of the antisymmetric matrix corresponding to the Lie algebra is the rotation matrix.

[0119] S35, the error uncertainty matrix P in the navigation domain n , converted to the error uncertainty matrix P in the runway domain r .

[0120] Specifically, S35 includes:

[0121] S351, Solve P n The eigenvalue d n and the eigenvector v n .

[0122] S352. Use formula: dr=d n Solving for Pr The eigenvalue d r and the eigenvector v r .

[0123] S353, using formula P r =v r d r (v r ) -1 , solve the error uncertainty matrix P in the runway domain r .

[0124] S4: If the camera captures the visual image at the current moment, the visual measurement in the measurement domain is obtained by using the intelligent detection algorithm for the visual image at the current moment; based on the visual measurement, the system status in the runway domain is updated. and its uncertainty matrix If the camera does not capture the visual image at the current moment, return to step S2.

[0125] Among them, intelligent detection algorithms include methods such as neural networks.

[0126] Specifically, S4 includes:

[0127] S41 determines a measurement domain according to sensor data of the inertial vision integrated navigation system.

[0128] Specifically, the measurement domain refers to the measurement results of the sensor, which involves the collection and processing of sensor data. It includes visual sensors, radio altimeters, GNSS, inertial navigation and other devices that capture environmental information. The measurement results of radio altimeters, GNSS, inertial navigation and other devices are information such as position and speed, while visual sensors provide pixel-level feature information. There is a big difference in the information types of the two. Since the measurement result processing of radio altimeters, GNSS, inertial navigation and other devices is relatively mature, the measurement domain of visual sensors is mainly discussed here. The measurement results of the camera are dimensionless pixel-level features, which are based on the pixel coordinate system. The pixel coordinate system is set to the S system, with the origin located in the upper left corner of the image, the positive direction of the x-axis to the right along the horizontal direction of the image, and the positive direction of the y-axis to the bottom along the vertical direction. Since the image is a 2D plane, it has no z-axis, only x and y axes. During the imaging process, the camera reduces the three-dimensional plane to a two-dimensional plane, losing the depth result along the camera optical axis. See for details. Figure 3 .

[0129] S42, obtain the full amount of aircraft position and velocity information under the navigation system from the external inertial navigation high frequency. Among them, the inertial navigation solution position is p n .

[0130] S43, acquiring the image information and the corresponding timestamp obtained by the visual sensor at a lower frequency, and using the intelligent detection algorithm to obtain the visual measurement results θ1, θ2, u, v.

[0131] Among them, θ1, θ2 represent the pixel slopes of two feature lines, and u, v represent the pixel coordinates of a feature point.

[0132] S44, respectively determine the predicted values ​​of the visual measurement results θ1, θ2, u, and v

[0133] The prediction of point features can be expressed as:

[0134]

[0135] The prediction of line features can be expressed as:

[0136] l=(K[RT])L(K[RT]) T

[0137] In the above formula, M c is the position coordinate of the feature point in the camera system, M r is the position coordinate of the feature point in the runway system, [RT] represents the rotation and translation relationship of projecting the point coordinates from the runway system to the camera system, K is the internal parameter, s is the normalization parameter, L represents the Plücker matrix of the three-dimensional line, and l represents the Plücker matrix of the two-dimensional line.

[0138] S45. According to the predicted value And visual measurement results visual measurement results θ1, θ2, u, v, using Determine the total filter measurement y.

[0139] S46. Solve the filter observation matrix H according to the database information of the inertial vision integrated navigation system and the full amount of posture and velocity information of the aircraft under the navigation system.

[0140] Specifically, S46 includes:

[0141] S461, according to the rotation matrix from n system to c system Determine the parameter r 11 , r 12 ,...r 33 .

[0142]

[0143] S462, according to δγ r , using the formula θa=δγ r Solve for the parameters θ and a.

[0144] It should be noted that a is δγr A normalized direction vector with a modulus of 1, θ is δγ r The mold length.

[0145] S463, according to the parameters θ, a, using the formula Determine the parameter J.

[0146] S464, based on the database information of the inertial vision integrated navigation system, the full position and speed information of the aircraft under the navigation system, and the actual runway domain position M of the feature point to be observed r , determine the camera system position of the feature point to be observed

[0147]

[0148] in, The rotation matrix from the track system to the camera system.

[0149] S465, calculate the position p based on the inertial navigation n , the rotation matrix from the navigation system to the camera system State δp n and the camera and the aircraft's boom arm Using the formula Determine the runway coordinates of the current camera position

[0150] S466, according to parameter r 11 , r 12 ,...r 33 , runway width b, determine parameters a1~a4.

[0151] a1=r 11 r 22 r 33 -r 11 r 23 r 32 -r 12 r 21 r 33 +r 12 r 23 r 31 +r 13 r 21 r 32 -r 13 r 22 r 31

[0152] a2=r 11 r 32 br 12 r 31 b

[0153]

[0154] S467, determine the parameter f according to the camera internal parameter K x 、f y .

[0155] S468, according to parameters a1~a4, f x 、f y , the runway coordinates of the current camera position Parameter r 11 , r 12 ,...r 33 Determine the parameters h11, h12, h21, h22.

[0156]

[0157] S469, according to parameters h11, h12, h21, h22, f x 、f y , J, camera position of the feature point to be observed The actual runway domain position M of the feature point to be observed r , the rotation matrix from the track system to the camera system Solve for the matrix H.

[0158]

[0159] S48 performs status update and obtains the updated runway domain status and its runway domain uncertainty matrix

[0160]

[0161] In the above formula, R is the visual observation error, which is given by the detection algorithm, and P′ is the covariance matrix before the runway domain state is updated. To measure the updated runway domain status, is the uncertainty matrix of the runway domain state before measurement update, is the uncertainty matrix of the runway domain state after measurement update.

[0162] S5, the runway domain status and its uncertainty matrix Transition to navigation domain state and its uncertainty matrix Thereby achieving inertial and visual combined navigation.

[0163] Connect all the above results in series, and the overall filtering algorithm flow is shown in the following figure. Figure 7In the figure, the red lines represent the branches that are running at any continuous moment, and the green lines represent the data branches that are running only when there is observation. At any moment, the time update occurs in the navigation domain state, and is projected into the runway domain state through the conversion relationship described by S3. When there is observation, the measurement update occurs in the runway domain state, and the updated state and uncertainty matrix are projected into the navigation domain state through the inter-domain conversion relationship described by S5. This cycle repeats.

[0164] Embodiment 2

[0165] The present application provides an inertial vision integrated navigation method based on multi-domain conversion, comprising the following steps:

[0166] S1. Obtain the database information of the inertial vision integrated navigation system, including various parameters of the inertial navigation, various parameters of the camera, calibration parameters of the camera and the aircraft, calibration parameters of the inertial navigation and the aircraft, latitude and longitude of the target runway entrance, runway direction, runway length and width, flight trajectory and other information.

[0167] Specifically, S1 includes the following steps:

[0168] Obtain inertial navigation parameters. According to the selected inertial navigation signal, check the manual to obtain the zero bias, random walk, zero-position first-order Markov parameters of the inertial navigation gyro and the zero bias, random walk, zero-position first-order Markov parameters of the inertial navigation adder, and the inertial navigation update frequency f ins .

[0169] Camera parameter acquisition. Including camera intrinsic parameter K, camera distortion parameter C, camera sampling frequency f vis .

[0170] Determine the calibration parameters of the camera and the body, including the translation vector from the camera system to the origin of the body system The rotation matrix from the camera system to the camera system Determine the calibration parameters of the camera and inertial navigation, including the translation vector from the camera system to the origin of the inertial system The rotation matrix from the camera system to the inertial system

[0171] Determine the target runway information, including the latitude and longitude of the runway entrance point lon0, lat0, height0, the runway heading angle β, the runway length l and width b. Determine a landing trajectory that eventually lands 400m from the runway entrance. Determine the runway system coordinates M of the observation point feature r =[500, 20, 0] T .

[0172] S2, initializing the system error state in the navigation domain, and updating the system state in the navigation domain according to the time update principle of the system in the navigation domain.

[0173] Specifically, S2 includes:

[0174] S21. Determine the navigation domain of the inertial vision integrated navigation system according to the WGS 84 coordinate system, the local navigation system, the aircraft system and the camera system.

[0175] S22, determining the navigation domain state x:

[0176] x=[δp n ,δv n ,δγ n , b g , b s ] T

[0177] In the above formula, δp n represents the position error in the local navigation system, δv n represents the velocity error in the local navigation system, δγ n Represents the misalignment angle error in the local navigation system.

[0178] From the definition of misalignment angle, we know that b g represents the gyro's zero bias error in frame b, b a Represents the zero bias error of the accelerometer in frame b.

[0179] S23, Initialize the navigation domain state. OK

[0180] x0=[10 10 10 5 5 5 0.01 0.001 0.001 0.00001 0.00001 0.00001 0.000010.00001 0.0

[0181] Initialize a reasonable navigation domain state based on the inertial navigation parameters in the database information.

[0182] S24, according to the time update principle of the system under the navigation domain, the navigation domain state x at the kth moment k Update the time.

[0183] In the present invention, time update always occurs in the navigation domain system state, and measurement update always occurs in the runway domain state. Time update refers to the update matrix Φ of the high-precision inertial navigation. k , which will not be elaborated here. Based on this, the time update equation of the navigation domain state is obtained:

[0184]

[0185] In the above formula, represents the predicted value of the navigation domain state at time k+1, w k is white noise.

[0186] S3, converts the navigation domain state and uncertainty matrix into the runway domain state and uncertainty matrix.

[0187] Specifically, S3 includes:

[0188] S31. Determine a runway domain according to a target runway coordinate system.

[0189] The runway domain is based on the target runway coordinate system. The feature results obtained from the measurement will be converted to the runway domain to obtain the position of the target carrier relative to the runway system. Generally, the runway is regarded as a horizontal plane rectangle. When obtaining the prior data of the runway, it is necessary to obtain the longitude and latitude coordinates of the runway entrance point (or other deterministic description position), the runway orientation, the length and width of the runway, the horizontal inclination angle of the runway, and other data to determine the position and angle of the runway system in the geocentric coordinate system. The runway system is set to the r system, which is defined as: the center of the runway entrance in the current flight direction is the origin of the runway system, the z direction is perpendicular to the runway plane and points to the center of the earth, the x direction is along the runway and points to the tail end of the runway, and the y direction is perpendicular to the runway direction, following the right-hand rule. See for details. Figure 4 .

[0190] S32. Determine the runway domain state χ′ according to the user's concerns and visual measurement characteristics:

[0191] x′=[δp r ,δγ r ] T

[0192] In the above formula, δp r represents the position error of the system projection in the runway system, δγ r It represents the angular error of the system projection in the runway system.

[0193] S33, the system position error δp in the navigation domain n , converted to the system position error δp in the runway domain r In particular, when the rotation matrix from the navigation system to the runway system is hour:

[0194]

[0195] Among them, δp r represents the position error projection of the system in the r system, δp n Represents the position error projection of the system in the n system.

[0196] S34, the system angle error δγ in the navigation domain n , converted to the system angle error δγ in the runway domain r .

[0197] Specifically, S34 includes:

[0198] Using the formula The system angle error δγ in the navigation domain n , converted to the system angle error δγ in the runway domain r In particular, when the rotation matrix from the navigation system to the runway system is When δγ r =δγ n .

[0199] S35, the error uncertainty matrix P in the navigation domain n , converted to the error uncertainty matrix P in the runway domain r In particular, when the rotation matrix from the navigation system to the runway system is When P r =P n .

[0200] S4: If the camera captures the visual image at the current moment, the visual measurement in the measurement domain is obtained by using the intelligent detection algorithm for the visual image at the current moment; based on the visual measurement, the system status in the runway domain is updated. and its uncertainty matrix If the camera does not capture the visual image at the current moment, return to step S2.

[0201] Among them, intelligent detection algorithms include methods such as neural networks.

[0202] Specifically, S4 includes:

[0203] S41 determines a measurement domain according to sensor data of the inertial vision integrated navigation system.

[0204] Specifically, the measurement domain refers to the measurement results of the sensor, which involves the collection and processing of sensor data. It includes visual sensors, radio altimeters, GNSS, inertial navigation and other devices that capture environmental information. The measurement results of radio altimeters, GNSS, inertial navigation and other devices are information such as position and speed, while visual sensors provide pixel-level feature information. There is a big difference in the information types of the two. Since the measurement result processing of radio altimeters, GNSS, inertial navigation and other devices is relatively mature, the measurement domain of visual sensors is mainly discussed here. The measurement results of the camera are dimensionless pixel-level features, which are based on the pixel coordinate system. The pixel coordinate system is set to the S system, with the origin located in the upper left corner of the image, the positive direction of the x-axis to the right along the horizontal direction of the image, and the positive direction of the y-axis to the bottom along the vertical direction. Since the image is a 2D plane, it has no z-axis, only x and y axes. During the imaging process, the camera reduces the three-dimensional plane to a two-dimensional plane, losing the depth result along the camera optical axis. See for details. Figure 3 .

[0205] S42, obtain the full amount of aircraft position and velocity information under the navigation system from the external inertial navigation high frequency. Among them, the inertial navigation solution position is p n =[-1000, 0, -35] T .

[0206] S43, acquiring the image information and the corresponding timestamp obtained by the visual sensor at a lower frequency, and using the intelligent detection algorithm to obtain the visual measurement results θ1, θ2, u, v.

[0207] Among them, θ1, θ2 represent the pixel slopes of two feature lines, and u, v represent the pixel coordinates of a feature point.

[0208] S44, respectively determine the predicted values ​​of the visual measurement results θ1, θ2, u, and v

[0209] The prediction of point features can be expressed as:

[0210]

[0211] The prediction of line features can be expressed as:

[0212] l=(K[RT])L(K[RT]) T In the above formula, M c is the position coordinate of the feature point in the camera system, M r is the position coordinate of the feature point in the runway system, [RT] represents the rotation and translation relationship of projecting the point coordinates from the runway system to the camera system, K is the internal parameter, s is the normalization parameter, L represents the Plücker matrix of the three-dimensional line, and l represents the Plücker matrix of the two-dimensional line.

[0213] S45. According to the predicted value And visual measurement results visual measurement results θ1, θ2, u, v, using Determine the total filter measurement y.

[0214] S46. Solve the filter observation matrix H according to the database information of the inertial vision integrated navigation system and the full amount of posture and velocity information of the aircraft under the navigation system.

[0215] Specifically, S46 includes:

[0216] S461, according to the rotation matrix from n system to c system Determine the parameter r 11 , r 12 ,...r 33 .

[0217]

[0218] S462, according to δγr , using the formula θa=δγ r Solve for the parameters θ and a.

[0219] It should be noted that a is δγ r A normalized direction vector with a modulus of 1, θ is δγ r The mold length.

[0220] S463, according to the parameters θ, a, using the formula Determine the parameter J.

[0221] S464, calculate the position p based on inertial navigation n , the full amount of aircraft posture, speed information and the actual runway domain position of the feature point to be observed under the navigation system r , determine the camera position of the feature point to be observed In particular, when the rotation matrix from the track system to the camera system is hour,

[0222]

[0223] S465, calculate the position p according to the inertial navigation n , the rotation matrix from the navigation system to the camera system State δp n and the camera and the aircraft's boom arm Using the formula Determine the runway coordinates of the current camera position In particular, when the rotation matrix from the navigation system to the runway system hour,

[0224] S466, according to parameter r 11 , r 12 ,...r 33 , runway width b, determine parameters a1~a4.

[0225] a1=r 11 r 22 r 33 -r 11 r 23 r 32 -r 12 r 21 r 33 +r 12 r 23 r 31 +r 13 r 21 r 32 -r 13 r 22 r 31

[0226] a2=r 11 r 32 br 12 r 31 b

[0227]

[0228] S467, determine the parameter f according to the camera internal parameter K x 、f y .

[0229] S468, according to parameters a1~a4, f x 、f y , the runway coordinates of the current camera position Parameter r 11 , r 12 ,...r 33 Determine the parameters h11, h12, h21, h22.

[0230]

[0231]

[0232] S469, according to parameters h11, h12, h21, h22, f x 、f y , J, camera position of the feature point to be observed The actual runway domain position M of the feature point to be observed r , the rotation matrix from the track system to the camera system Solve for the matrix H.

[0233]

[0234] S48 performs status update and obtains the updated runway domain status and its runway domain uncertainty matrix

[0235]

[0236] In the above formula, R is the visual observation error, which is given by the detection algorithm, and P′ is the covariance matrix before the runway domain state is updated. To measure the updated runway domain status, is the uncertainty matrix of the runway domain state before measurement update, is the uncertainty matrix of the runway domain state after measurement update.

[0237] S5, the runway domain status and its uncertainty matrix Transition to navigation domain state and its uncertainty matrix

[0238] Concatenating all the above results, Figure 7 In the figure, the red lines represent the branches that are running at any continuous moment, and the green lines represent the data branches that are running only when there is an observation. At any moment, the time update occurs in the navigation domain state, and is projected into the runway domain state through the conversion relationship described by S3. When there is an observation, the measurement update occurs in the runway domain state, and the updated state and uncertainty matrix are projected into the navigation domain state through the inter-domain conversion relationship described by S5. This cycle repeats.

[0239] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. An inertial vision integrated navigation method based on multi-domain conversion, characterized in that: Methods include: S1. Obtaining database information of the inertial vision integrated navigation system; S2, initializing the system error state in the navigation domain, and updating the system state in the navigation domain according to the time update principle of the system in the navigation domain; S3, converting the navigation domain state and uncertainty matrix into the runway domain state and uncertainty matrix; S4, if the camera captures the visual image at the current moment, the visual measurement in the measurement domain is obtained by using the intelligent detection algorithm for the visual image at the current moment; Update the system status under the runway domain based on the visual measurement and its uncertainty matrix If the camera does not capture the visual image at the current moment, return to step S2; S5, the runway domain status and its uncertainty matrix Transition to navigation domain state and its uncertainty matrix Thereby achieving inertial and visual combined navigation.

2. The inertial vision integrated navigation method according to claim 1, characterized in that S2 include: S21. Determine the navigation domain of the inertial vision integrated navigation system according to the WGS 84 coordinate system, the local navigation system, the aircraft system and the camera system; S22, determining the navigation domain state x: x=[δp n ,δv n ,dg n ,b g ,b a ] T In the above formula, δp n represents the position error in the local navigation system, δv n represents the velocity error in the local navigation system, δγ n Represents the misalignment angle error in the local navigation system; S23, initializing the navigation domain state x0; S24, according to the time update principle of the system under the navigation domain, the navigation domain state x at the kth moment k Update the time.

3. The inertial vision integrated navigation method according to claim 1, characterized in that S3 include: S31. Determine a runway domain according to a target runway coordinate system; S32. Determine the runway domain state x′ according to the user's concerns and visual measurement characteristics: x′=[δp r ,dg r ] T In the above formula, δp r represents the position error of the system projection in the runway system, δγ r It represents the angular error of the system projection under the runway system; S33, the system position error δγ in the navigation domain n , converted to the system position error δγ in the runway domain r ; in, is the rotation matrix from the navigation system to the runway system, δp r represents the position error projection of the system in the r system, δp n represents the position error projection of the system in the n-frame; S34, the system angle error δγ in the navigation domain n , converted to the system angle error δγ in the runway domain r ; S35, the error uncertainty matrix P in the navigation domain n , converted to the error uncertainty matrix P in the runway domain r .

4. The inertial vision integrated navigation method according to claim 3, characterized in that: S34 includes: Using the formula The system angle error δγ in the navigation domain n , converted to the system angle error δγ in the runway domain r ; Among them, exp((δγn ) ^) is the rotation vector δγ n The corresponding rotation matrix, exp((δγ r ) r ) is the rotation vector δγ r The corresponding rotation matrix is Represents the rotation matrix from n system to r system.

5. The inertial vision integrated navigation method according to claim 3, characterized in that: S35 includes: S351, Solve P n The eigenvalue d n and the eigenvector v n ; S352. Use formula: d r =d n Solving for P r The eigenvalue d r and the eigenvector v r ; S353, using formula P r =v r d r (v r ) -1 , solve the error uncertainty matrix P in the runway domain r .

6. The inertial vision integrated navigation method according to claim 1, characterized in that S4 include: S41 determines a measurement domain according to sensor data of an inertial vision integrated navigation system; S42, obtain the full amount of aircraft position and velocity information under the navigation system from the external inertial navigation high frequency, where the inertial navigation solution position is p n ; S43, acquiring the image information and the corresponding timestamp obtained by the visual sensor at a lower frequency, and using the intelligent detection algorithm to obtain the visual measurement results θ1, θ2, u, v; wherein θ1, θ2 represent the pixel slopes of the two feature lines, and u, v represent the pixel coordinates of a feature point; S44, respectively determine the predicted values ​​of the visual measurement results θ1, θ2, u, and v S45. According to the predicted value And visual measurement results visual measurement results θ1, θ2, u, v, using Determine the total filter measurement y; S46, solving the filter observation matrix H according to the database information of the inertial vision integrated navigation system and the full amount of posture and velocity information of the aircraft under the navigation system; S47 performs status update and obtains the updated runway domain status and its runway domain uncertainty matrix 7. The inertial vision integrated navigation method according to claim 6, characterized in that: S46 includes: S461, according to the rotation matrix from n system to c system Determine the parameter r 11 , r 12 ,...r 33 ; S462, according to δγ r , using the formula θa=δγ r Solve for parameters θ and a; S463, according to the parameters θ, a, using the formula Determine parameter J; S464, based on the database information of the inertial vision integrated navigation system, the full position and speed information of the aircraft under the navigation system, and the actual runway domain position M of the feature point to be observed r , determine the camera system position of the feature point to be observed in, is the rotation matrix from the runway system to the camera system; S465, calculate the position p based on the inertial navigation n , the rotation matrix from the navigation system to the camera system State δp n and the camera and the aircraft's boom arm Using the formula Determine the runway coordinates of the current camera position S466, according to parameter r 11 , r 12 ,...r 33 , runway width b, determine parameters a1~a4; a1＝r 11 r 22 r 33 -r 11 r 23 r 32 -r 12 r 21 r 33 +r 12 r 23 r 31 +r 13 r 21 r 32 -r 13 r 22 r 31 ； a2=r 11 r 22 b-r 12 r 21 b; S467, determine the parameter f according to the camera internal parameter K x 、f y ; S468, according to parameters a1~a4, f x 、f y , the runway coordinates of the current camera position Parameter r 11 , r 12 ,...r 33 Determine the parameters h11, h12, h21, h22; S469, according to parameters h11, h12, h21, h22, f x 、f y , J, camera position of the feature point to be observed The actual runway domain position M of the feature point to be observed r , the rotation matrix from the track system to the camera system Solve the matrix H; 8. The inertial vision integrated navigation method according to claim 6, characterized in that: Step 44 includes: The prediction of point features can be expressed as: The prediction of line features can be expressed as: l=(K[RT])L(K[RT]) T In the above formula, M c is the position coordinate of the feature point in the camera system, M r is the position coordinate of the feature point in the runway system, [RT] represents the rotation and translation relationship of projecting the point coordinates from the runway system to the camera system, K is the internal parameter, s is the normalization parameter, L represents the Plücker matrix of the three-dimensional line, and l represents the Plücker matrix of the two-dimensional line.

9. An inertial vision integrated navigation device based on multi-domain conversion, characterized in that: The device is used to implement the inertial vision integrated navigation method based on multi-domain conversion as described in claim 1.

Images