Error correction method for inertial navigation system

Abstract

The invention relates to an error correction method for an inertial navigation system. Inertial navigation data and data of an auxiliary sensor are collected in real time; calibrating a transverse central axis and a longitudinal central axis of the carrier, determining a first calibration point through the transverse central axis and the longitudinal central axis, and generating a carrier ball according to the first calibration point; converting the attitude angle into a spherical coordinate according to the collected attitude angle data; predicting position coordinates of the next moment through a prediction model of the inertial navigation system, and meanwhile, obtaining actually measured sphere center coordinates; fusing the predicted sphere center coordinate and the actually measured sphere center coordinate to obtain a fused sphere center coordinate; and calculating a difference vector between the fusion carrier ball and the actually measured carrier ball, determining a correction vector according to the calculated difference vector, and gradient-limiting an error range. The accuracy and real-time performance of the inertial navigation system in continuous high-speed acquisition of high-precision navigation positioning and attitude angle information are improved by fusing predicted and actually measured data and using a correction vector to finely adjust the position.

Description

Technical Field

[0001] The present invention relates to the technical field of inertial navigation, and in particular to an error correction method for an inertial navigation system. Background Art

[0002] With the rapid development of navigation technology, inertial navigation systems (INS) have become indispensable positioning and attitude perception devices in many carriers (such as unmanned aerial vehicles, vehicles, ships, etc.). However, traditional inertial navigation systems rely on integral operations to calculate the position, speed, and attitude of the carrier, so there is a problem that the cumulative error increases with time, which seriously affects the accuracy and reliability of navigation. To overcome this defect, researchers have proposed various error correction methods, among which the correction technology based on multi-sensor data fusion has attracted much attention due to its high efficiency and accuracy.

[0003] For example, Chinese Patent Publication No. CN 118518101 B2 discloses an error correction method, system, device, and product for an inertial navigation system. The method therein includes: obtaining in real time the inertial navigation measurement data collected by the inertial navigation system in a specified carrier; obtaining in real time the auxiliary observation data collected by a variety of auxiliary sensors in the specified carrier to obtain a variety of auxiliary observation data, and performing fusion processing on the variety of auxiliary observation data to obtain the fused auxiliary observation data; based on the inertial navigation measurement data and the fused auxiliary observation data, obtaining the state error value of the specified carrier through an extended Kalman filter; and performing error correction on the inertial navigation system according to the state error value to obtain the corrected state information of the specified carrier.

[0004] The comparative document uses a variety of auxiliary observation data and utilizes data fusion technology for error correction. Most of the fusion methods have problems such as high computational complexity, poor real-time performance, and high requirements for hardware, which limit their application in resource-constrained environments (such as small unmanned aerial vehicles). Summary of the Invention

[0005] Aiming at the technical problems existing in the prior art, the present invention improves the estimation accuracy of the carrier position and attitude, and improves the accuracy and real-time performance of the inertial navigation system when continuously and rapidly obtaining high-precision navigation positioning and attitude angle information by fusing predicted and measured data, and using a correction vector to fine-tune the position.

[0006] The technical solution for the present invention to solve the above technical problems is as follows: An error correction method for an inertial navigation system, S101, collecting inertial navigation data and data of auxiliary sensors in real time; S102, calibrating the lateral central axis and longitudinal central axis of the carrier, determining a first calibration point through the lateral central axis and longitudinal central axis, and generating a carrier sphere according to the first calibration point; S103. Convert the attitude angle to spherical coordinates according to the attitude angle data collected in step S101; S104. Predict the position coordinates at the next moment through the prediction model of the inertial navigation system. At the same time, obtain the measured spherical center coordinates; S105. Fuse the predicted spherical center coordinates and the measured spherical center coordinates obtained in step S104 to obtain the fused spherical center coordinates; S106. Calculate the difference vector between the fused carrier sphere and the measured carrier sphere, determine the correction vector according to the calculated difference vector, and limit the error range of the gradient based on steps S101 to S106.

[0007] Preferably, the transverse central axis is the axis of the carrier in the traveling direction, which is consistent with the moving direction of the carrier. The longitudinal central axis is perpendicular to the transverse central axis. The intersection point of the transverse central axis and the longitudinal central axis, compared with the center of gravity of the carrier, the coincidence point of the intersection point and the center of gravity or the intersection point closest to the center of gravity is used as the first calibration point. The first calibration point is located on the transverse central axis and has an intersection point with the longitudinal central axis.

[0008] Preferably, the attitude angle is converted to spherical coordinates through a rotation matrix. The rotation matrix = , where are the rotation matrices around the Z-axis, Y-axis, and X-axis respectively, = , = , = .

[0009] Preferably, use the weighted calculation formula to fuse the predicted coordinates ( , , ) and the measured coordinates ( ), and the specific formula is as follows: , , , to obtain the fused spherical center coordinates ( , , ).

[0010] Preferably, the steps of limiting the error range of the gradient: S201. Mark the second calibration point on the carrier. The physical distance between the second calibration point and the first calibration point is constant; use the first camera to obtain the images of the drone at consecutive moments; S202. Detect the positions of the first calibration point and the second calibration point; S203. Calculate the angle between the balance line at the current moment and the balance line at the next moment; S204. Correct the attitude angle error of the UAV according to the included angle calculated in step S203.

[0011] Preferably, for the current moment, calculate the balance line vector between the first calibration point and the second calibration point =( ), for the next moment, calculate the balance line vector between the first calibration point and the second calibration point , convert the balance line vector into a unit vector and , , , where and respectively represent the magnitudes of the vectors and .

[0012] Preferably, the specific steps for setting the second camera based on the first camera in step S201: S301. Set the second camera on the UAV, collect data through the camera, and calculate the attitude angle and position change value of the UAV; S302. Integrate the calculated angle information with the measurement data of the inertial navigation system to correct the error of the inertial navigation system.

[0013] Preferably, the shooting direction of the second camera is different from that of the first camera. The shooting direction of the first camera is parallel to the horizontal central axis, and the shooting direction of the second camera is perpendicular to the horizontal central axis.

[0014] Preferably, the steps for the carrier sphere and image fusion: S401. Collect the image sequence in real time and fuse the collected images with the carrier sphere; S402. Identify and track the calibration points, and calculate the coordinates of the center of the carrier sphere according to the change trajectory of the calibration points; S403. Calculate, predict and correct the process noise.

[0015] Preferably, use the process equation = + , where is the state information of the specified carrier at the next moment k + 1, is the current state information of the specified carrier at the current moment k, is the inertial navigation measurement data at the current moment k, is the current state information of the specified carrier at the current moment k and the inertial navigation measurement data at the current moment k, is the preset first process noise. By solving the process equation, the predicted value of the carrier center - of - sphere coordinates at the next moment is obtained.

[0016] The beneficial effects of the present invention are as follows: By fusing predicted and measured data and using a correction vector to finely adjust the position, the estimation accuracy of the carrier position and attitude is improved. Steps such as spherical - coordinate conversion and fusion calculation are added, but the computational complexity of the spherical - coordinate conversion and fusion - calculation steps is relatively small and will not significantly affect the real - time nature of the data. Compared with high - computational - complexity models or formulas, this solution improves the accuracy while maintaining real - time performance, and improves the accuracy and real - time performance of the inertial navigation system when continuously and rapidly obtaining high - precision navigation positioning and attitude - angle information. By using image - fusion technology to more accurately calibrate the center - of - sphere coordinates and using the error at the current moment to correct the predicted data at the next moment, the navigation accuracy of the inertial navigation system can be significantly improved. At the same time, the complex calculation and model - verification processes are avoided, and the data processing at the next moment is directly guided by the error at the current moment, thereby improving the real - time performance of the system. Since the error in the predicted data is corrected in real time, the data between consecutive moments has higher coherence, which helps the unmanned aerial vehicle to fly stably in a dynamic environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 is a schematic flow chart of an error - correction method for an inertial navigation system according to the present invention; Figure 2 is a schematic flow chart of the gradient - limited error range in an embodiment of the present invention; Figure 3 is a schematic flow chart of setting a second camera based on the first camera in step S201 in an embodiment of the present invention; Figure 4 is a schematic flow chart of the carrier sphere and image fusion in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0018] Next, the technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative efforts fall within the scope of protection of the present application.

[0019] In the description of the present application, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, features defined with "first" and "second" may explicitly or implicitly include one or more of the said features. In the description of the present application, the meaning of "a plurality" is two or more unless otherwise specifically defined.

[0020] In the description of the present application, the term "for example" is used to mean "serving as an example, illustration, or explanation". Any embodiment described as "for example" in the present application is not necessarily construed as being more preferred or having more advantages than other embodiments. The following description is provided to enable any person skilled in the art to implement and use the present invention. In the following description, details are set forth for purposes of explanation. It should be understood that those of ordinary skill in the art can recognize that the present invention can be implemented without using these specific details. In other instances, well-known structures and processes are not elaborated in detail to avoid obscuring the description of the present invention with unnecessary details. Therefore, the present invention is not intended to be limited to the embodiments shown, but is consistent with the broadest scope that conforms to the principles and features disclosed in the present application.

[0021] Embodiment 1: Figure 1 is a schematic flow diagram of an inertial navigation system error correction method according to Embodiment 1 of the present invention, including the following steps: S101, collect inertial navigation data and data of auxiliary sensors in real time; By starting the inertial navigation system, the acceleration and angular velocity of the carrier are recorded in real time. At the same time, a barometer, a magnetometer, and an optical sensor (such as a camera or LiDAR) are started to record relevant data in real time.

[0022] S102, calibrate the lateral central axis and the longitudinal central axis of the carrier, determine a first calibration point through the lateral central axis and the longitudinal central axis, and generate a carrier sphere according to the first calibration point; Specifically, the lateral central axis is the axis of the carrier in the traveling direction, which is consistent with the movement direction of the carrier. The longitudinal central axis is perpendicular to the lateral central axis. The intersection point of the lateral central axis and the longitudinal central axis is compared with the center of gravity of the carrier. The coincidence point of the intersection point and the center of gravity or the intersection point closest to the center of gravity is used as the first marking point. The first marking point is located on the lateral central axis and has an intersection with the longitudinal central axis. Taking the first marking point as the center of the sphere and the distance between the first calibration point and the farthest stationary point on the carrier as the radius, the farthest stationary point refers to the point that remains unchanged in position relative to the center of the sphere (the first calibration point) and is the farthest from the center of the sphere during the movement of the carrier. By using computer-aided design software (CAD), a three-dimensional sphere model is generated with the first calibration point as the center of the sphere and the distance between the first calibration point and the farthest stationary point on the carrier as the radius. This sphere model is the carrier sphere.

[0023] S103. Convert the attitude angles into spherical coordinates according to the attitude angle data collected in step S101. Specifically, the attitude angles include the roll angle Φ, the pitch angle θ, and the yaw angle ψ. The roll angle Φ is the angle of rotation around the longitudinal axis (Y-axis) of the carrier, the pitch angle θ is the angle of rotation around the transverse axis (X-axis) of the carrier, and the yaw angle ψ is the angle of rotation around the vertical axis (Z-axis) of the carrier. The attitude angles are converted into spherical coordinates through a rotation matrix. Let the spherical coordinates of the first calibration point on the carrier sphere be (ρ, θ', φ'), where ρ is the distance to the center of the sphere (equal to the radius R), and θ' and φ' are the polar angle and azimuth angle in the spherical coordinate system. Since the center of the sphere coincides with the first calibration point, ρ is always R, and the rotation matrix = , where are the rotation matrices around the Z-axis, Y-axis, and X-axis respectively. = , = , = . θ' is obtained by calculating the ratio between the Z component z' of the rotated vector and the magnitude of the vector, and then taking the inverse cosine function: θ' = arccos( ).

[0024] φ' is obtained by calculating the X component x' and Y component y' of the rotated vector. Specifically, the ratio between y' and x' can be calculated using the arctangent function: φ′ = arctan2(y′, x′).

[0025] S104. Predict the position coordinates at the next moment through the prediction model of the inertial navigation system. At the same time, obtain the measured coordinates of the center of the sphere.

[0026] Specifically, use the prediction model of the inertial navigation system. The prediction model is the state transition equation. Based on the current position coordinates and attitude angle data, predict the position coordinates at the next moment. The coordinate data of the carrier is measured in real time through sensors. The coordinate data includes information such as longitude, latitude, and altitude. The collected position coordinate data is converted into a unified format.

[0027] S105. Fuse the predicted center-of-sphere coordinates and the measured center-of-sphere coordinates obtained in step S104 to obtain the fused center-of-sphere coordinates.

[0028] Furthermore, use the weighted calculation formula to fuse the predicted coordinates ( , , ) and the measured coordinates ( ). The specific formula is as follows: , , , to obtain the fused center-of-sphere coordinates ( , , ).

[0029] S106. Calculate the difference vector between the fused carrier sphere and the measured carrier sphere, and determine the correction vector according to the calculated difference vector.

[0030] Specifically, calculate the position differences between the fused carrier sphere and the measured carrier sphere in the x, y, and z directions, i.e., Δx = - , Δy = - , Δz = - ; Set the correction ratio β, 0 < β < 1, correction vector = (βΔx, βΔy, βΔz), where βΔx is the correction component in the x direction, βΔy is the correction component in the y direction, and βΔz is the correction vector in the z direction. = + , = + , where , , are the positions of the fused carrier sphere after correction in the x, y, and z directions respectively, , and are the positions of the fused carrier sphere before correction in the x, y, and z directions respectively. Update the calculated corrected position information to the inertial navigation system to complete the error correction.

[0031] The technical solutions in the embodiments of the present application at least have the following technical effects or advantages: By fusing predicted and measured data and using the correction vector to fine-tune the position, the estimation accuracy of the carrier position and attitude is improved. Steps such as spherical coordinate transformation and fusion calculation are added, but the computational complexity of the spherical coordinate transformation and fusion calculation steps is relatively small and will not have a significant impact on the real-time nature of the data. Compared with high-computational models or formulas, this solution improves the accuracy while maintaining real-time performance, and improves the accuracy and real-time nature of the inertial navigation system when continuously and rapidly obtaining high-precision navigation positioning and attitude angle information.

[0032] Embodiment 2: Based on Embodiment 1, in this embodiment, the possible range of gradient-limited errors is determined, and errors are adjusted and selected in real time to ensure coherence, accuracy, and timeliness of the overall data. The carrier is limited to an unmanned aerial vehicle.

[0033] Steps for determining the gradient-limited error range: S201. Mark a second calibration point on the carrier, with a constant physical distance between the second calibration point and the first calibration point; obtain images of the drone at consecutive moments.

[0034] Specifically, mark the second calibration point on the side of the fuselage or other parts that are easy to identify and stable, ensuring that the position of the second calibration point will not be affected by air flow interference or other external factors during the movement of the drone.

[0035] Fix a first camera on the carrier, use time-lapse photography technology to obtain images of the drone at consecutive moments, start the flight process of the drone, and start capturing images at consecutive moments to obtain overlapping images of the current moment and the next moment. The overlapping images of the current moment and the next moment are achieved by adjusting the viewing angle and focal length of the camera and superimposing the images of the two moments. Add the center point of the sphere at the current moment and the longitudinal and lateral central axes to the image at the current moment, and the center point of the sphere is located at the center of the image or the center position of the drone.

[0036] S202. Detect the positions of the first calibration point and the second calibration point; Specifically, load the images taken at the current moment and the next moment into the image processing software OpenCV. In the image processing software, use the weighted average method to convert each pixel point of the color image into a grayscale value. Use the feature point detection algorithm SIFT to detect the first calibration point and the second calibration point in the image and output the feature point coordinates. When detecting the feature points, ensure that the center coordinates of the sphere in the image at the next moment overlap with the center coordinates of the sphere in the image at the current moment to accurately obtain the position changes of the first and second calibration points. The SIFT algorithm will traverse each pixel or pixel block of the image and determine whether the position is a feature point according to the gradient change criterion. Use the Sobel operator to calculate the gradients of the image in the x direction and the y direction, and use the Euclidean norm G = sqrt to combine the gradients in the x direction and the y direction to calculate the gradient magnitude G. The gradient direction θ is obtained by calculating arctan(Gy / Gx). Perform non-maximum suppression on the gradient magnitude image, that is, compare each pixel point with the pixel points in its neighborhood and only retain those pixel points with the maximum gradient magnitude in their neighborhood as candidate feature points. Finally, the SIFT algorithm will output the coordinates of a series of calibration points.

[0037] S203. Calculate the angle between the balance line at the current moment and the balance line at the next moment; Further, for the current moment (denoted as t) and the next moment (denoted as t + 1), respectively determine the pixel coordinates of the first marker point and the second marker point and represent them as ( ), ), and ( ), ). For the current moment, calculate the balance line vector between the first calibration point and the second calibration point =( ) For calculating the balance line vector between the first calibration point and the second calibration point at the next moment Convert the balance line vector into a unit vector and , , where and respectively represent the magnitudes of the vectors and . Perform a dot product operation on the converted unit vectors, dot_product = Use the vector dot product and the cosine function to calculate the angle θ between the two balance lines. The cosine value of the angle θ is cos(θ) = dot_product, θ = arccos(cos(θ)), and the calculated angle θ is obtained.

[0038] S204. Correct the attitude angle of the UAV according to the angle calculated in step S203; In some embodiments, a set angle threshold is 5 degrees. At a certain moment, the calculated angle θ is 3 degrees, which is less than the set threshold of 5 degrees, and the attitude of the UAV is stable and no correction is required. Due to the influence of sudden strong winds, the attitude of the UAV changes. Calculate the angle θ again, and the result obtained at this time is 7 degrees, which is greater than the set threshold of 5 degrees, indicating a significant change in the attitude of the UAV. Next, use geometric relationships, the angle θ, and the physical distance d between the marked points (these marked points may be specific sensors or reference points on the UAV), combined with the data of the inertial sensor, to calculate the actual attitude angle change of the UAV. The pitch angle of the UAV increases by 6 degrees, and the roll angle decreases by 3 degrees. Compare the calculated actual attitude angle change with the attitude angle provided by the inertial navigation system. If a difference is found between the two, it indicates that there may be an error in the inertial navigation system. According to the comparison result, correct the inertial navigation system by adjusting the calibration parameters of the sensor or using a filter to smooth the data to more accurately reflect the actual attitude angle change of the UAV.

[0039] The technical solutions in the above embodiments of the present application have at least the following technical effects or advantages: By calibrating the second marked point on the UAV and using time-lapse photography technology to obtain images at consecutive moments, calculate the change in the angle between the connecting lines of the marked points, so as to adjust and select errors in real time to ensure the coherence, accuracy, and timeliness of the overall data. By directly measuring the position change of the marked points on the UAV, the attitude angle information of the UAV can be obtained more accurately. By setting a reasonable angle threshold, the attitude change of the UAV can be monitored in real time and adjusted when necessary, thereby reducing errors.

[0040] Embodiment 3: On the basis of Embodiment 2, this embodiment adds a second camera for time-lapse photography. From another angle, the shooting direction of the first camera is parallel to the horizontal central axis, and the shooting direction of the second camera is perpendicular to the horizontal central axis. In this way, the obtained angle information is more complete.

[0041] Specific steps for setting the second camera: S301. Set a second camera on the drone, collect data through the camera, and calculate the attitude angle and position change values of the drone. Specifically, the shooting direction of the set second camera is different from that of the first camera. The shooting direction of the first camera is parallel to the horizontal central axis, and the shooting direction of the second camera is perpendicular to the horizontal central axis. Set an exposure time of two seconds, keep the shutter of the camera open, record all the movement trajectories of the drone and the marking points within two seconds, generate an image with a multi-frame superposition effect, use an image processing algorithm for motion detection to identify the movement trajectories in the image. Motion detection can identify areas with obvious brightness changes in the image, and the areas with obvious brightness changes are the movement trajectories of the drone and the marking points. Use a tracking algorithm, Kalman filtering, to further smooth and track. According to the results of trajectory detection, extract the movement trajectories of the drone and the marking points from the image.

[0042] The extracted trajectory data is represented as a series of position points in a time series. Use triangulation to reconstruct the three-dimensional coordinates of the marking points, and calculate the attitude angle of the drone relative to the marking points according to the reconstructed three-dimensional coordinates.

[0043] S302. Integrate the calculated angle information with the measurement data of the inertial navigation system to correct the errors of the inertial navigation system. Furthermore, adjust the angle information and the measurement data of the inertial navigation system to be synchronized in time, record the time stamp corresponding to each angle data, and match the generated time stamp with the time stamp of the inertial navigation system; use the mathematical model of the inertial navigation system to predict the state vector at the next moment based on the current state estimation and input control quantities. According to the characteristics of the process noise and sensor noise of the system model, calculate the covariance matrix of the predicted state error, calculate the Kalman gain according to the predicted error covariance and measurement noise covariance, compare the state error value calculated using the extended Kalman filter with a set threshold, and if the state error value exceeds the set threshold, it indicates that there are significant errors in the measurement data of the inertial navigation system. Correct the state of the inertial navigation system according to the calculated angle information, GPS data or other reliable data.

[0044] The technical solutions in the embodiments of the present application at least have the following technical effects or advantages: More abundant angular information is obtained from two different perspectives, and the attitude and position changes of the drone can be calculated more accurately, thereby improving the navigation accuracy of the inertial navigation system. Even if a certain camera or sensor fails or malfunctions, the system can still perform error correction because another camera provides angular information.

[0045] Embodiment 4: Based on Embodiment 3, in this embodiment, the image is fused with the carrier sphere to obtain a continuous sphere of the change trajectories of the first and second marked points of the carrier sphere in the time-lapse image, with the aim of further calibrating the accuracy of the coordinates of the sphere center.

[0046] Steps for fusing the image with the carrier sphere: S401, Continuously acquire an image sequence and fuse the acquired image with the carrier sphere; Specifically, use the feature extraction algorithm SIFT to identify the feature points in the image. The specific identification method is described in detail in step S202 and will not be elaborated in this embodiment. The extracted feature points include corner points, edges, or regions with rich texture in the image. Calculate a feature descriptor for each feature point, determine a local region around the feature point, calculate the image gradient of this region, divide the local region into multiple small sub-regions (called cells), and calculate the histogram of the gradient direction within each cell. Concatenate the gradient direction histograms of all cells to form a long feature vector, which is the descriptor of the feature point. Calculate the similarity between the feature descriptor in the image and the feature descriptor in the 3D model through the Euclidean distance, compare the calculated similarity with a preset similarity threshold, match the image feature descriptor corresponding to the similarity greater than the similarity threshold with the feature descriptor in the 3D model. After the matching is completed, fuse the 3D model with the image by mapping the texture of the 3D model onto the image or overlaying the contour of the 3D model on the image.

[0047] S402, Identify and track the calibration points, and calculate the coordinates of the center of the carrier sphere according to the change trajectories of the calibration points; Specifically, in the fused image, according to the position information of the marked points in the 3D model, feature detection is used to search for the corresponding calibrated points, and the position change trajectory of the calibrated points is obtained in real time in consecutive image frames. In the first frame image, feature detection is used to determine the initial positions of the first marked point and the second marked point. In subsequent frames, the optical flow field is calculated, and the motion vectors corresponding to the initial marked point positions are found. Finally, the positions of the marked points are updated according to the motion vectors, and the motion state of the calibrated points is estimated by calculating the motion vectors in the optical flow field. The tracked calibrated points are converted into actual three-dimensional space coordinates through image correction technology, and the parameters between the calibrated points are extracted. The parameters include the distance between the calibrated points, the included angle of the connecting lines, and the included angle between these connecting lines and the normal of the carrier sphere surface, etc. Based on the extracted geometric parameters, a geometric model of the carrier sphere and its center of the sphere is constructed, and according to the geometric relationship and the position information of the marked points, the coordinates of the center of the sphere are solved through mathematical calculations.

[0048] S403, calculate, predict and correct the process noise; Specifically, obtain the coordinates of the center of the carrier sphere obtained through the measured parameters at the current moment, calculate the difference between the measured coordinates at the current moment and the calibrated coordinates (image fusion coordinates) after image fusion as the current process noise , and use the process equation = + , where is the state information of the specified carrier at the next moment k + 1, is the current state information of the specified carrier at the current moment k, is the inertial measurement data at the current moment k, is the current state information of the specified carrier at the current moment k and the inertial measurement data at the current moment k is the state transition equation, is the preset first process noise. By solving the process equation, the predicted value of the coordinates of the center of the carrier sphere at the next moment is obtained. Compare the predicted coordinates with the coordinates of the center of the carrier sphere obtained through actual measurement at the next moment. If the difference between the predicted coordinates and the actual measurement coordinates is large, there is an error in the prediction model, and it is necessary to re-evaluate and adjust. According to the recalculated process noise and actual measurement data, the prediction model is corrected. By continuously iterating the above steps, the prediction model is gradually optimized to more accurately reflect the actual motion state of the center of the carrier sphere. The iteration process can continue until the prediction error reaches an acceptable range.

[0049] The technical solutions in the above embodiments of the present application at least have the following technical effects or advantages: By using image fusion technology to more accurately calibrate the center coordinates of the sphere and using the error at the current moment to correct the prediction data for the next moment, the navigation accuracy of the inertial navigation system can be significantly improved. At the same time, it avoids the complex calculation and model verification processes, directly uses the error at the current moment to guide the data processing for the next moment, thereby improving the real-time performance of the system. Since the error in the prediction data is corrected in real time, the data between consecutive moments has higher coherence, which helps the drone to fly stably in a dynamic environment.

[0050] It should be noted that in the above embodiments, the descriptions of each embodiment have their own emphases. For the parts not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0051] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) containing computer-usable program code.

[0052] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to the embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram can be implemented by computer program instructions, and the combination of the processes and / or blocks in the flowchart and / or block diagram can also be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded computer, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate means for realizing the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0053] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured article including instruction means, and the instruction means realizes the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0054] These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus, such that a series of operational steps are performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing steps for implementing the functions specified in one flow Figure 1 one or more flows and / or boxes Figure 1 or boxes specified in one or more boxes.

[0055] Although the preferred embodiments of the present invention have been described, additional changes and modifications can be made by those skilled in the art once they learn of the basic inventive concept. Therefore, the appended claims are intended to be construed to cover the preferred embodiments as well as all changes and modifications falling within the scope of the present invention.

[0056] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims

1. An error correction method for an inertial navigation system, characterized in that S101, real-time collect inertial navigation data and data of auxiliary sensors; S102, calibrate the lateral central axis and longitudinal central axis of the carrier, determine the first calibration point through the lateral central axis and longitudinal central axis, and generate a carrier sphere according to the first calibration point; S103, convert the attitude angle into spherical coordinates according to the attitude angle data collected in step S101; S104, predict the position coordinates at the next moment through the prediction model of the inertial navigation system, and at the same time, obtain the measured spherical center coordinates; S105, fuse the predicted spherical center coordinates and the measured spherical center coordinates obtained in step S104 to obtain the fused spherical center coordinates; S106, calculate the difference vector between the fused carrier sphere and the measured carrier sphere, determine the correction vector according to the calculated difference vector, and limit the error range based on steps S101 to S106.

2. The error correction method of an inertial navigation system according to claim 1, characterized in that The lateral central axis is the axis of the carrier in the traveling direction, which is consistent with the movement direction of the carrier. The longitudinal central axis is perpendicular to the lateral central axis. The intersection point of the lateral central axis and the longitudinal central axis, compared with the center of gravity of the carrier, the coincidence point of the intersection point and the center of gravity or the intersection point closest to the center of gravity is used as the first calibration point. The first calibration point is located on the lateral central axis and has an intersection with the longitudinal central axis.

3. A method for error correction of an inertial navigation system according to claim 1, characterized in that, Convert the attitude angle to spherical coordinates through the rotation matrix. The rotation matrix = , where are the rotation matrices about the Z-axis, Y-axis, and X-axis respectively, = , = , = .

4. A method for correcting errors in an inertial navigation system according to claim 1, characterized in that, Use a weighted calculation formula to fuse the predicted coordinates ( , , ) and the measured coordinates ( ). The specific formula is as follows: , , , and the fused center coordinates of the sphere are obtained ( , , ).

5. A method for correcting errors in an inertial navigation system according to claim 1, characterized in that, Steps for limiting the error range by gradient: S201, mark a second calibration point on the carrier, and the physical distance between the second calibration point and the first calibration point is constant; use the first camera to obtain images of the drone at consecutive moments; S202, detect the positions of the first calibration point and the second calibration point; S203, calculate the angle between the balance line at the current moment and the balance line at the next moment; S204, correct the error of the attitude angle of the drone according to the angle calculated in step S203.

6. The error correction method of an inertial navigation system according to claim 5, characterized in that, Calculate the balance line vector between the first calibration point and the second calibration point for the current moment =( ) and calculate the balance line vector between the first calibration point and the second calibration point for the next moment , convert the balance line vector into a unit vector and , , , where and represent the magnitudes of the vectors and respectively.

7. A method for correcting errors in an inertial navigation system according to claim 5, characterized in that, Specific steps for setting the second camera based on the first camera in step S201: S301, set the second camera on the drone, collect data through the camera, and calculate the values of the attitude angle and position change of the drone; S302, fuse the calculated angle information with the measurement data of the inertial navigation system to correct the error of the inertial navigation system.

8. A method for correcting errors in an inertial navigation system according to claim 7, characterized in that The shooting direction of the second camera is different from that of the first camera. The shooting direction of the first camera is parallel to the lateral central axis, and the shooting direction of the second camera is perpendicular to the lateral central axis.

9. A method for correcting errors in an inertial navigation system according to claim 1, characterized in that, Steps for fusing the carrier sphere and the image: S401, real-time collect an image sequence, and fuse the collected image with the carrier sphere; S402, identify and track the calibration points, and calculate the coordinates of the center of the carrier sphere according to the change trajectory of the calibration points; S403, calculate, predict and correct the process noise.

10. A method for correcting errors in an inertial navigation system according to claim 9, characterized in that, Using the process equation = + , where is the state information of the specified carrier at the next moment k + 1, is the current state information of the specified carrier at the current moment k, is the inertial navigation measurement data at the current moment k, is the current state information of the specified carrier at the current moment k and the inertial navigation measurement data at the current moment k is the state transition equation, is the preset first process noise. By solving the process equation, the predicted value of the carrier center - of - sphere coordinates at the next moment is obtained.

Images