A calibration method and positioning method of an inertial navigation and laser speedometer combined navigation positioning system and related devices
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- BEIHANG UNIV
- Filing Date
- 2026-03-12
- Publication Date
- 2026-05-29
Smart Images
Figure CN122108203A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of calibration technology for integrated navigation and positioning systems, and in particular to a calibration method, positioning method and related device for an inertial navigation and laser velocimeter integrated navigation and positioning system. Background Technology
[0002] Laser Doppler velocimeters operate based on the Doppler effect of light waves, offering advantages such as high precision, non-contact operation, and rapid response. Combining a strapdown inertial navigation system (SINS) with a laser Doppler velocimeter enables high-precision navigation through inertial units and velocity measurement, suitable for land vehicle navigation scenarios. The navigation and positioning accuracy of this combined system depends on the precision of the selected sensors and also on the accurate calibration of various error parameters.
[0003] In existing calibration methods, Kalman filtering is generally used to combine the estimation of the instrument error parameters and navigation error states to be calibrated, transforming the instrument calibration problem into a state estimation problem. When there are many error terms, the calibration process becomes very complex, and the various errors cannot be completely decoupled. Summary of the Invention
[0004] The purpose of this application is to provide a calibration method, positioning method, and related devices for a combined inertial navigation and laser velocimeter navigation and positioning system, which can reduce the complexity of the calibration process.
[0005] To achieve the above objectives, this application provides the following solution: In a first aspect, this application provides a calibration method for an inertial navigation and laser velocimeter integrated navigation and positioning system, the inertial navigation and laser velocimeter integrated navigation and positioning system including an inertial navigation module and a laser velocimeter that can be mounted on a moving target; the calibration method is used to determine the error parameters of the inertial navigation and laser velocimeter integrated navigation and positioning system; the error parameters include the scale factor error of the laser velocimeter, and the pitch angle installation error, heading angle installation error, and heading angle alignment error of the inertial navigation module; the calibration method includes: Acquire test data of the moving target body during the calibration process; the test data includes the moving speed of the moving target body measured by the laser velocimeter, the calibration starting point position in the navigation coordinate system, the position of the landmark point, and the attitude calculation results and position of the inertial navigation module at the landmark point position.
[0006] Based on the velocity of the moving target, the calibration starting position in the navigation coordinate system, and the attitude calculation results of the inertial navigation module, the dead reckoning position in the navigation coordinate system is calculated.
[0007] Calculate the ratio of the first distance to the second distance, and calculate the scale factor error of the laser velocimeter based on the ratio; the first distance is determined based on the calibration starting point position and the landmark position; the second distance is determined based on the calibration starting point position and the dead reckoning position.
[0008] Based on the calibration starting point position, the landmark position, and the dead reckoning position, the pitch angle installation error of the inertial navigation module is calculated.
[0009] Based on the calibration starting point position, the inertial navigation module position, and the dead reckoning position, calculate the heading angle installation error of the inertial navigation module.
[0010] Based on the calibration starting point position, the landmark position, the dead reckoning position, and the heading angle installation error of the inertial navigation module, the heading angle alignment error of the inertial navigation module is calculated.
[0011] Secondly, this application provides a positioning method for a combined inertial navigation and laser velocimeter navigation and positioning system, including: Obtain the error parameters of the inertial navigation and laser velocimeter integrated navigation and positioning system; the error parameters include the scale factor error of the laser velocimeter, and the pitch angle installation error, heading angle installation error, and heading angle alignment error of the inertial navigation module; the error parameters are determined based on the calibration method of the inertial navigation and laser velocimeter integrated navigation and positioning system described above.
[0012] Obtain the dead reckoning position of the moving target in the navigation coordinate system at the previous moment.
[0013] The system acquires the current velocity of the moving target measured by the laser velocimeter, the attitude transformation matrix of the inertial navigation module, and the pitch and yaw angles output by the inertial navigation module.
[0014] Based on the dead reckoning position of the moving target in the navigation coordinate system at the previous moment, the error parameters, the velocity of the moving target measured by the laser velocimeter at the current moment, the attitude transformation matrix of the inertial navigation module, and the pitch and heading angles output by the inertial navigation module, the dead reckoning position of the moving target in the navigation coordinate system at the current moment is calculated.
[0015] Thirdly, this application provides a computer device, including: a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the processor executes the computer program to implement the steps of the calibration method or positioning method of the inertial navigation and laser velocimeter integrated navigation and positioning system described above.
[0016] Fourthly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the calibration method or positioning method of the inertial navigation and laser velocimeter integrated navigation and positioning system described above.
[0017] Fifthly, this application provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the calibration method or positioning method of the inertial navigation and laser velocimeter integrated navigation and positioning system described above.
[0018] According to the specific embodiments provided in this application, this application has the following technical effects: This application provides a calibration method, a positioning method, and related apparatus for an inertial navigation and laser velocimeter integrated navigation and positioning system. The system includes an inertial navigation module and a laser velocimeter installed on a moving target. The calibration method determines the error parameters of the system. These parameters include the laser velocimeter's calibration factor error, and the inertial navigation module's pitch angle installation error, heading angle installation error, and heading angle alignment error. The calibration method includes: acquiring experimental data of the moving target during the calibration process; the experimental data includes the moving target's velocity measured by the laser velocimeter, the calibration starting point position in the navigation coordinate system, the landmark position, and the inertial navigation module's attitude calculation results and position at the landmark position; and calculating the dead reckoning position in the navigation coordinate system based on the moving target's velocity, the calibration starting point position in the navigation coordinate system, and the inertial navigation module's attitude calculation results. Based on the above data and calculation results, the location information (including latitude, longitude, and altitude) of the calibration start point, landmark point (i.e., the actual end point), and dead reckoning can be determined. This application has theoretically demonstrated that the scale factor error of the laser velocimeter can be reflected in the plane displacement direction of the inertial navigation and laser velocimeter combined navigation and positioning system, the heading angle alignment error can be reflected in the direction perpendicular to the plane displacement direction and altitude direction of the navigation system, and the pitch angle installation error can be reflected in the altitude direction of the navigation system. Therefore, the scale factor error of the velocimeter can be calculated by calculating the ratio of the first distance to the second distance and based on this ratio. The pitch angle installation error and heading angle installation error can be calculated by using data with landmark information (calibration start point position, landmark point position, and dead reckoning position) and data without landmark information (calibration start point position, inertial navigation module position, and dead reckoning position), respectively. The heading angle alignment error can be calculated by combining data with and without landmark information. The entire calculation process relies solely on the known calibration starting point position, landmark position, and measurement and calculation data from the inertial navigation and laser velocimeter combined navigation and positioning system (the motion velocity of the moving target measured by the laser velocimeter, the attitude calculation results of the inertial navigation module, and the position of the inertial navigation module) for direct calculation. It does not require the establishment of complex state equations, thus reducing the complexity of the calibration process. Attached Figure Description
[0019] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0020] Figure 1 This is an application environment diagram of a calibration method for a combined inertial navigation and laser velocimeter navigation and positioning system according to an embodiment of this application; Figure 2 A flowchart illustrating a calibration method for a combined inertial navigation and laser velocimeter navigation and positioning system provided in an embodiment of this application; Figure 3 This is a schematic diagram illustrating the relationship between the navigation coordinate system and the displacement coordinate system in one embodiment of this application; Figure 4 This is a schematic diagram of the calibration path in one embodiment of this application; Figure 5 This is a simulation diagram of the vehicle's driving trajectory in one embodiment of this application; Figure 6 A schematic diagram showing the calibration results of the inertial navigation module's heading angle installation error; Figure 7 A schematic diagram showing the calibration results of the laser velocimeter's scale factor error. Figure 8 This is a schematic diagram showing the calibration results of the heading angle alignment error of the inertial navigation module. Figure 9 A schematic diagram showing the calibration results under different initial latitude and longitude errors; Figure 10 A schematic flowchart illustrating a positioning method for a combined inertial navigation and laser velocimeter navigation and positioning system provided in an embodiment of this application; Figure 11 This is a schematic diagram of the structure of a computer device provided in an embodiment of this application.
[0021] Attached image labels: 102 terminal, 104 server. Detailed Implementation
[0022] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.
[0023] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0024] The calibration method for the inertial navigation and laser velocimeter combined navigation and positioning system provided in this application embodiment can be applied to, for example... Figure 1In the application environment shown, terminal 102 communicates with server 104 via a network. A data storage system can store the data that server 104 needs to process. The data storage system can be set up independently, integrated into server 104, or placed in the cloud or on other servers. Terminal 102 can send the test data of the moving target during the calibration process to server 104. After receiving the test data of the moving target during the calibration process, server 104, based on the moving target's velocity, the calibration starting position in the navigation coordinate system, and the attitude calculation results of the inertial navigation module, calculates the dead reckoning position in the navigation coordinate system; calculates the ratio of a first distance to a second distance, and calculates the calibration factor error of the laser velocimeter based on the ratio; the first distance... The first distance is determined based on the calibration starting point position and the landmark position; the second distance is determined based on the calibration starting point position and the dead reckoning position; the pitch angle installation error of the inertial navigation module is calculated based on the calibration starting point position, the landmark position, and the dead reckoning position; the heading angle installation error of the inertial navigation module is calculated based on the calibration starting point position, the inertial navigation module position, and the dead reckoning position; the heading angle alignment error of the inertial navigation module is calculated based on the calibration starting point position, the landmark position, the dead reckoning position, and the heading angle installation error of the inertial navigation module. The server 104 can feed back the obtained laser velocimeter scale factor error, as well as the pitch angle installation error, heading angle installation error, and heading angle alignment error of the inertial navigation module to the terminal 102. In addition, in some embodiments, the calibration method of the inertial navigation and laser velocimeter combined navigation and positioning system can also be implemented separately by the server 104 or the terminal 102. For example, the terminal 102 can directly perform calibration processing on the test data of the moving target body to be processed during the calibration process, or the server 104 can obtain the test data of the moving target body to be processed during the calibration process from the data storage system and perform calibration processing on the test data of the moving target body to be processed during the calibration process.
[0025] The terminal 102 can be, but is not limited to, various desktop computers, laptops, smartphones, tablets, IoT devices, and portable wearable devices. IoT devices can include smart speakers, smart TVs, smart air conditioners, and smart in-vehicle devices. Portable wearable devices can include smartwatches, smart bracelets, and head-mounted devices. The server 104 can be implemented using a standalone server or a server cluster composed of multiple servers, or it can be a cloud server.
[0026] In one exemplary embodiment, such as Figure 2As shown, a calibration method for an inertial navigation and laser velocimeter integrated navigation and positioning system is provided. The integrated navigation and positioning system includes an inertial navigation module and a laser velocimeter that can be installed on a moving target. The calibration method is used to determine the error parameters of the integrated navigation and positioning system. The error parameters include the scale factor error of the laser velocimeter, and the pitch angle installation error, heading angle installation error, and heading angle alignment error of the inertial navigation module.
[0027] (1) Model the relationship between the position error of the navigation coordinate system and the parameters that need to be calibrated.
[0028] The coordinate system is defined as follows: Carrier coordinate system or inertial navigation coordinate system (b system): using This indicates that the origin is the center of gravity of the carrier. The shaft moves to the right along the transverse axis of the carrier. The shaft moves forward along the longitudinal axis of the carrier. The axis is upward along the vertical axis of the carrier, and it is assumed that the sensitive center of the inertial navigation module coincides with the center of gravity of the carrier.
[0029] Vehicle body coordinate system (m-system): using This indicates that the origin is the center of gravity of the vehicle body. The axle moves to the right along the transverse axis of the vehicle body. The axle moves forward along the longitudinal axis of the vehicle body. The axle moves upwards along the vertical axis of the vehicle body.
[0030] Navigation coordinate system (n-system): using This indicates that it is the coordinate system used by the inertial navigation system when solving for navigation parameters, and the East-North-Sky (ENU) geographic coordinate system is selected as the navigation coordinate system.
[0031] Define the mounting angle between the inertial navigation module and the vehicle body. . , , These represent the angles of rotation about the z-axis, x-axis, and y-axis of the m-system, respectively.
[0032] The installation angle error of the inertial navigation module is represented by a vector. Among them, the installation angle error in the roll direction It does not affect the solution, and can be Consider it as zero.
[0033] Due to the installation angle error of the inertial navigation module (heading angle installation error) Installation error of pitch angle and attitude error And the scale factor error of the laser velocimeter. The actual velocity of the moving target (i.e., the carrier, which is a vehicle in this embodiment) in the navigation coordinate system is: ; in, This represents the projection of the vehicle's velocity, taking error into account, onto the n-frame. This represents the attitude transformation matrix from the b-frame to the n-frame, taking into account the error. This represents the attitude transformation matrix from the m-frame to the b-frame, taking into account the error. This represents the projection of the vehicle's velocity, taking error into account, onto the m-frame. yes The cross product matrix; This represents the attitude transformation matrix from the b-system to the n-system; This indicates the speed of the carrier as measured by the laser velocimeter; This represents the overall scale factor of the laser velocimeter's output. This indicates the scale factor error of the laser velocimeter.
[0034] Expanding the above equation and ignoring higher-order minor quantities, we get: ; ; ; ; in, These are intermediate symbols used to facilitate the expression of definitions in formulas; they have no actual physical meaning. This represents the projection of the vehicle's velocity onto the n-frame.
[0035] set up , All angles are small; the vehicle's pitch and roll angles vary near zero. After inertial navigation self-alignment, the horizontal attitude error is very small. Simplifying the above equation, we get: ; in, This indicates the heading angle alignment error.
[0036] Multiplying both sides by time t, we get: ; in, It is the vehicle's displacement vector in the navigation coordinate system, including horizontal displacement (including x-axis and y-axis directions) and vertical displacement; The distance traveled by the vehicle can be obtained from the vehicle's odometer, or, if the vehicle calibration process only involves straight-line travel, it can be approximated as the distance between two points (the calibration starting point and the road sign point).
[0037] This represents the vehicle's displacement error in the navigation coordinate system. This represents the vehicle's displacement in the x-direction within the navigation coordinate system. This represents the vehicle's displacement in the y-direction within the navigation coordinate system. This represents the vehicle's displacement in the height direction (z-axis) within the navigation coordinate system.
[0038] Define a displacement coordinate system k, where its y-axis points in the direction of vehicle displacement, its z-axis coincides with the z-axis of the navigation coordinate system, and its x-axis follows the right-hand rule with the y and z axes. The relationship between the displacement coordinate system k and the navigation coordinate system n is as follows: Figure 3 As shown, by transforming the position error of the navigation coordinate system to the displacement coordinate system, the above equation can be written as: ; in, This represents the displacement error of the vehicle in the displacement coordinate system. The displacement of the vehicle in the y-axis direction of the displacement coordinate system.
[0039] As shown in the above formula, after transforming the position error of the navigation coordinate system to the displacement coordinate system, the scale factor error of the laser velocimeter can be reduced. The installation error of the inertial navigation module's heading angle And the pitch angle installation error of the inertial navigation module The impact on positional error is reflected in the longitudinal, lateral, and vertical directions, respectively.
[0040] (2) Based on the above conclusions, the position error of the navigation coordinate system is calibrated, and the calibration path with landmarks is determined, such as... Figure 4 As shown.
[0041] The vehicle travels a distance, and point A in the diagram is the designated starting point. The latitude, longitude, and altitude are known. B is a landmark point (i.e., the endpoint of the marker), with known latitude, longitude, and altitude. The latitude and longitude of the landmark point are... C represents the dead reckoning position corresponding to the designated endpoint, including latitude, longitude, and altitude. The dead reckoning latitude and longitude are... D represents the location of the inertial navigation module corresponding to the calibration endpoint, with latitude and longitude of 10 ... .
[0042] In another embodiment, without a precise odometer, the distance traveled is equal to the distance traveled by driving in a straight line from A to B; the distance traveled is recorded as... .
[0043] (3) Perform error calibration.
[0044] The calibration method is executed by a computer device, specifically by a terminal or server alone, or by both a terminal and a server. In this embodiment, the method is applied to... Figure 1 Taking server 104 as an example, the explanation includes the following steps 201 to 208. Wherein: Step 201: Obtain test data of the moving target during the calibration process; the test data includes the moving speed of the moving target measured by the laser velocimeter, the calibration starting point position in the navigation coordinate system, the attitude calculation results of the inertial navigation module, and the position of the inertial navigation module at the landmark point.
[0045] Step 202: Based on the motion velocity of the moving target, the calibration starting position in the navigation coordinate system, and the attitude calculation results of the inertial navigation module, calculate the dead reckoning position in the navigation coordinate system.
[0046] Step 203: Calculate the ratio of the first distance to the second distance, and calculate the scale factor error of the laser velocimeter based on the ratio; the first distance is determined based on the calibration starting point position and the landmark position; the second distance is determined based on the calibration starting point position and the dead reckoning position.
[0047] Step 204: Calculate the pitch angle installation error of the inertial navigation module based on the calibration starting point position, the landmark position, and the dead reckoning position.
[0048] Step 205: Calculate the heading angle installation error of the inertial navigation module based on the calibration starting point position, the inertial navigation module position, and the dead reckoning position.
[0049] Step 206: Calculate the heading angle alignment error of the inertial navigation module based on the calibration starting point position, the landmark position, the dead reckoning position, and the heading angle installation error of the inertial navigation module.
[0050] In some embodiments, step 201 specifically includes: Step 2011: Determine the calibration route. To ensure calibration accuracy, the coordinates of the starting point and the ending point (landmark points) of the calibration route must be known precisely. Obtain the Earth's longitude, latitude, and altitude coordinates (coordinates in the navigation coordinate system) of the starting point and the ending point.
[0051] Step 2012: The vehicle travels from the calibration starting point to the calibration endpoint. During the calibration process, data output from the inertial navigation module and laser velocimeter is continuously collected. The specific data content and format are as follows: Inertial navigation module output data includes gyro angular velocity measurement results, accelerometer acceleration measurement results, and attitude calculation results from the inertial navigation module (generally presented as attitude angles, i.e., pitch, roll, and yaw angles; the attitude transformation matrix can be obtained through simple calculations; the attitude calculation results are output in real time and participate in navigation calculations to ultimately obtain the positioning result), velocity (data in the navigation coordinate system), and position calculation results; laser velocimeter output data includes vehicle speed measurement results (data in the vehicle body coordinate system).
[0052] In some embodiments, the laser velocimeter is a laser Doppler velocimeter.
[0053] Step 2013: After the vehicle reaches the calibration endpoint, record the latitude and longitude calculated by the inertial navigation module as the position of the inertial navigation module.
[0054] In some embodiments, the calibration start point and calibration end point are driven as directly as possible to allow the distance between the start point and end point to be used as a substitute for the travel distance in the absence of an accurate odometer.
[0055] In some embodiments, in step 202, dead reckoning can be performed using the calibration starting point position, the attitude calculation results of the inertial navigation module, and the vehicle speed at each moment during the calibration process. The dead reckoning position corresponding to the endpoint is calculated, including longitude, latitude, and altitude. Currently, dead reckoning algorithms are relatively mature and will not be elaborated upon here.
[0056] In some embodiments, step 203, the calculation process of the scale factor error of the laser velocimeter includes: ; ; ; In the formula, The first distance ( Figure 4 (distance between points A and B) The second distance ( Figure 4 (distance between points A and C) This is the true scale factor of the laser velocimeter. This is the theoretical scale factor for the laser velocimeter. This represents the scale factor error of the laser velocimeter.
[0057] In some embodiments, in step 204, the formula for calculating the pitch angle installation error of the inertial navigation module is: ; In the formula, This is the difference between the altitude of the landmark location and the altitude of the dead reckoning location. The difference between the height of the starting point and the height of the landmark point; This refers to the scale factor error of the speedometer. To account for installation error in pitch angle, This refers to the distance traveled by the moving target during the calibration process.
[0058] ; ; for Figure 4 Height of point B for Figure 4 Height of point C for Figure 4 The height of point A in the middle.
[0059] The above formula shows that the pitch angle installation error of the inertial navigation module directly affects the altitude error. In some embodiments, if there are other altitude devices such as altimeters or GPS, the divergence of altitude error can be suppressed by damping, without calibrating the pitch angle installation error of the inertial navigation module in the above manner.
[0060] In some embodiments, for step 205, such as Figure 4 As shown, in a plane coordinate system, the angle between the line connecting two points and the x-axis can be represented by the arctangent of the difference between the coordinates of the two points. For example, the angle between AD and the x-axis can be represented as: .
[0061] Correspondingly, combined Figure 4 The formula for calculating the heading angle installation error (excluding singular intervals in different cases) is as follows: ; In the formula, For the heading angle installation error, To determine the latitude value of the starting point, To determine the longitude value of the starting point, The latitude value for dead reckoning. The longitude value for dead reckoning position. This represents the latitude value of the inertial navigation module's location. This represents the longitude value of the inertial navigation module's location.
[0062] In some embodiments, step 206 specifically includes: Step 2061: Based on the calibration starting point position, the landmark position, and the dead reckoning position, calculate the sum of the heading angle alignment error and the heading angle installation error of the inertial navigation module. The calculation formula (avoiding singular intervals depending on the case) is as follows: ; In the formula, For heading angle alignment error, For the heading angle installation error, To determine the latitude value of the starting point, To determine the longitude value of the starting point, This represents the latitude value of the landmark location. This represents the longitude value of the landmark location. The latitude value for dead reckoning. The longitude value used to calculate the position for dead reckoning.
[0063] Step 2062: Subtract the sum from the heading angle installation error to obtain the heading angle alignment error of the inertial navigation module.
[0064] Because navigation position error also includes heading angle installation error and heading angle alignment error Due to the influence of dead reckoning, when calculating the positioning error using the difference between the dead reckoning position and the landmark position, the dead reckoning positioning error obtained is actually the heading angle installation error. and heading angle alignment error The sum of these two elements does not fundamentally separate them.
[0065] Heading angle alignment error The attitude transformation matrix (transformation matrix from b-frame to n-frame) of the strapdown inertial navigation system affects the positioning results. Both the strapdown inertial navigation positioning position and the dead reckoning positioning position contain the influence of the attitude transformation matrix (transformation matrix from b-frame to n-frame). Therefore, when calculating the positioning error by subtracting the two, the positioning error caused by the heading angle alignment error can be canceled out, thereby separating the dead reckoning positioning error caused by the heading angle installation error of the inertial navigation module.
[0066] Subtracting the results of the formula in step 2061 and step 205 from the results above will give the heading angle alignment error. .
[0067] The above method, based on the accurate calibration of device errors (e.g., zero bias, scale factor error, and installation error of gyroscopes and accelerometers), calculates the scale factor error of the laser velocimeter, the installation angle error between the inertial navigation module and the vehicle body, and the heading angle alignment error. It also decouples the heading angle alignment error and the heading angle installation error. This method does not rely on complex path design and is of great significance for enhancing the environmental adaptability of the combined navigation and positioning system of the inertial navigation module and the laser velocimeter and improving the actual performance of the system.
[0068] In an exemplary embodiment, in order to verify the technical effect of the calibration method of the above embodiment, a simulation verification program was written using MATLAB. The program generates the vehicle trajectory and the sensor data under the trajectory through the trajectory generator program. The error estimation result is obtained through navigation calculation to evaluate the effect of the method.
[0069] trajectory as Figure 5 As shown, the vehicle accelerates horizontally and then travels at a constant speed of 20 m / s for 100 seconds. After that, it turns 90° to the right, then 90° to the left, and then continues to travel at a constant speed. The total travel time is 320 seconds.
[0070] The error sources are set as follows: the laser velocimeter's scale factor error is 2%, the inertial navigation module's heading angle installation error is 60'; the gyroscope and accelerometer zero bias error, as well as the initial alignment attitude and heading error, are set in the following three cases: Table 1. Experimental Groups and Data Set
[0071] Figure 6 The blue, red, and green lines represent the calibration results of the inertial navigation module's heading angle installation error in groups 1, 2, and 3 respectively using the markerless calibration method (corresponding to step 205). The unit is (′). The black line represents the calibration results of the inertial navigation module's heading angle installation error in group 1 using the marker-based calibration method (corresponding to step 206).
[0072] Figure 7 The blue, red, and green lines represent the calculated results of the scale factor error of the laser velocimeter under the no-marker calibration method in groups 1, 2, and 3, respectively, in units of (′). The black line represents the calculated result of the scale factor error of the laser velocimeter under the marker-based calibration method in group 1.
[0073] Figure 8 The blue, red, and green lines represent the heading angle alignment error calibration results of the no-landmark calibration method in groups 1, 2, and 3, respectively, in units of (′).
[0074] The positioning error of the inertial navigation module calibrated using the landmark location information (i.e. the error calculated in step 206) includes the heading angle alignment error, while the calibration accuracy of the positioning error of the inertial navigation module using the inertial navigation module position calibration method is not affected by the heading angle alignment error.
[0075] pass Figure 7 Simulation results of the calibration of the laser velocimeter's scale factor show that the error of the laser velocimeter's scale factor can be accurately calibrated by combining the two methods.
[0076] Figure 8The calibration results for the heading angle alignment error show that the calibration method combining landmark points and inertial navigation module position information can calibrate and correct the heading angle alignment error to a certain extent. The more accurate the calibration results for the inertial navigation module's heading angle installation error, the more accurate the calibration results for the heading angle alignment error.
[0077] In another embodiment, to examine the impact of the accuracy of the initial binding latitude and longitude on the calibration accuracy of the calibration method, the following experiment was designed and conducted under the following five conditions: Condition 1: Accurate latitude and longitude; Condition 2: Latitude and longitude error 10m; Condition 3: Latitude and longitude error 100m; Condition 4: Latitude and longitude error 1km; Condition 5: Latitude and longitude error 10km.
[0078] The gyroscope zero bias was set to 0.01° / h; the accelerometer zero bias was set to 50μg; the initial alignment attitude error was set to 10″; and the initial alignment heading error was set to 3'.
[0079] Test data such as Figure 9 As shown, by Figure 9 It can be seen that when the initial latitude and longitude error is within 1km, the accuracy of the inertial navigation module's heading angle installation error calibration is basically unaffected. Therefore, the landmark-free calibration method does not require knowing the accurate initial latitude and longitude; a rough initial latitude and longitude (within 1km) is sufficient.
[0080] Based on the above experimental results, the beneficial effects of the method proposed in this invention are as follows: High-precision calibration: It can accurately calibrate the heading angle installation error of the inertial navigation module (simulation shows the error is in the angular grade) and separate the heading angle alignment error, avoiding coupling problems associated with the landmark point method. The calibration error of the laser velocimeter's scale factor can be controlled within 1% (when the landmark point error is better than 2m). Initial latitude and longitude binding errors within 1km do not affect calibration accuracy.
[0081] High robustness: Reduces the impact of turning maneuvers through path design (such as straight road sections).
[0082] Efficiency Improvement: Simplifies the calibration process, reduces reliance on external equipment, supports real-time or near-real-time calibration, and improves the autonomy and reliability of the inertial navigation and laser velocimeter combined navigation and positioning system.
[0083] In another exemplary embodiment of this application, a positioning method for a combined inertial navigation and laser velocimeter navigation and positioning system is provided. The application environment of this method is similar to that of the calibration method, and it can also be applied to systems such as... Figure 1 The application environment shown will not be elaborated further here.
[0084] like Figure 10 As shown, the positioning method of the inertial navigation and laser velocimeter combined navigation and positioning system includes: Step 301: Obtain the error parameters of the inertial navigation and laser velocimeter integrated navigation and positioning system; the error parameters include the scale factor error of the laser velocimeter, and the pitch angle installation error, heading angle installation error, and heading angle alignment error of the inertial navigation module; the error parameters are determined based on the calibration method of the inertial navigation and laser velocimeter integrated navigation and positioning system described above.
[0085] Step 302: Obtain the dead reckoning position of the moving target in the navigation coordinate system at the previous moment.
[0086] Step 303: Obtain the current velocity of the moving target measured by the laser velocimeter, the attitude conversion matrix of the inertial navigation module, and the pitch and heading angles output by the inertial navigation module.
[0087] Step 304: Based on the dead reckoning position of the moving target in the navigation coordinate system at the previous moment, the error parameters, the velocity of the moving target measured by the laser velocimeter at the current moment, the attitude transformation matrix of the inertial navigation module, and the pitch and heading angles output by the inertial navigation module, calculate the dead reckoning position of the moving target in the navigation coordinate system at the current moment.
[0088] In some embodiments, there may be multiple waypoints.
[0089] During navigation, the moving target starts from the starting position. When it passes the first landmark, the starting position and the first landmark correspond to the calibration starting position and landmark position in step 201, respectively. Combined with the data from the laser velocimeter and the inertial navigation module, the error parameters are calibrated, and then the positioning method of the above embodiment is executed.
[0090] When passing the nth landmark (n≥2), the (n-1)th landmark and the nth landmark are used as the calibration starting point and landmark position in step 201, respectively. The error parameters are calibrated by combining the data from the laser velocimeter and the inertial navigation module, and then the positioning method of the above embodiment is executed.
[0091] In another exemplary embodiment, since the attitude transformation matrix (the transformation matrix from the b-system to the n-system) obtained by the gyroscope measurement is an important basic variable for the inertial navigation module to solve, compensating for the heading alignment error into the attitude transformation matrix from the b-system to the n-system can avoid amplifying the error in subsequent calculations, thereby improving the accuracy of navigation calculation.
[0092] The attitude angle is calculated from the gyroscope, but this will contain alignment errors. Assuming the horizontal attitude error of the inertial navigation module is very small after self-alignment, we have: ; .
[0093] The corresponding compensation process is as follows: .
[0094] In one exemplary embodiment, a computer device is provided, which may be a server or a terminal, and its internal structure diagram may be as follows. Figure 11 As shown, this computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and a database. The internal memory provides the environment for the operating system and computer programs stored in the non-volatile storage media. The database stores test data of the moving target during calibration, or error parameters of the inertial navigation and laser velocimeter combined navigation and positioning system, the dead reckoning position of the moving target in the navigation coordinate system at the previous moment, the velocity of the moving target measured by the laser velocimeter at the current moment, the attitude transformation matrix of the inertial navigation module (transformation matrix from b-frame to n-frame), and the pitch and yaw angles output by the inertial navigation module. The I / O interfaces are used for information exchange between the processor and external devices. The communication interface of this computer device is used to communicate with external terminals via a network. When the computer program is executed by the processor, it implements a calibration method for an inertial navigation and laser velocimeter integrated navigation and positioning system, or a positioning method for an inertial navigation and laser velocimeter integrated navigation and positioning system.
[0095] Those skilled in the art will understand that Figure 11 The structures shown are merely block diagrams of some structures related to the present application and do not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than shown in the figures, or combine certain components, or have different component arrangements. In an exemplary embodiment, a computer device is provided, including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps in the above-described method embodiments.
[0096] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.
[0097] In one exemplary embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.
[0098] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties. Moreover, the collection, use and processing of the relevant data are carried out in compliance with the relevant data protection laws and policies of the country where the location is located, and with the authorization granted by the owner of the corresponding device.
[0099] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments described above. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).
[0100] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.
[0101] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0102] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.
Claims
1. A calibration method for a combined inertial navigation and laser velocimeter navigation and positioning system, characterized in that, The inertial navigation and laser velocimeter integrated navigation and positioning system includes an inertial navigation module and a laser velocimeter that can be mounted on a moving target; the calibration method is used to determine the error parameters of the inertial navigation and laser velocimeter integrated navigation and positioning system; the error parameters include the scale factor error of the laser velocimeter, and the pitch angle installation error, heading angle installation error, and heading angle alignment error of the inertial navigation module; the calibration method includes: Acquire test data of the moving target during the calibration process; the test data includes the moving speed of the moving target measured by the laser velocimeter, the calibration starting point position in the navigation coordinate system, the position of the landmark point, the attitude calculation result of the inertial navigation module, and the position of the inertial navigation module at the landmark point; Based on the velocity of the moving target, the calibration starting position in the navigation coordinate system and the attitude calculation results of the inertial navigation module, the dead reckoning position in the navigation coordinate system is calculated. Calculate the ratio of the first distance to the second distance, and calculate the scale factor error of the laser velocimeter based on the ratio; the first distance is determined based on the calibration starting point position and the landmark position; the second distance is determined based on the calibration starting point position and the dead reckoning position. Based on the calibration starting point position, the landmark position, and the dead reckoning position, calculate the pitch angle installation error of the inertial navigation module; Based on the calibration starting point position, the inertial navigation module position, and the dead reckoning position, calculate the heading angle installation error of the inertial navigation module; Based on the calibration starting point position, the landmark position, the dead reckoning position, and the heading angle installation error of the inertial navigation module, the heading angle alignment error of the inertial navigation module is calculated.
2. The calibration method for the inertial navigation and laser velocimeter combined navigation and positioning system according to claim 1, characterized in that, The formula for calculating the scale factor error of the laser velocimeter is as follows: ； In the formula, The first distance, The second distance, This is the true scale factor of the laser velocimeter. This is the theoretical scale factor for the laser velocimeter. This represents the scale factor error of the laser velocimeter.
3. The calibration method for the inertial navigation and laser velocimeter combined navigation and positioning system according to claim 1, characterized in that, The formula for calculating the heading angle installation error of the inertial navigation module is as follows: ； In the formula, For the heading angle installation error, To determine the latitude value of the starting point, To determine the longitude value of the starting point, The latitude value for dead reckoning. The longitude value for dead reckoning position. This represents the latitude value of the inertial navigation module's location. This represents the longitude value of the inertial navigation module's location.
4. The calibration method for the inertial navigation and laser velocimeter combined navigation and positioning system according to claim 1, characterized in that, Based on the calibration starting point position, the landmark position, the dead reckoning position, and the heading angle installation error of the inertial navigation module, the heading angle alignment error of the inertial navigation module is calculated, specifically including: Based on the calibration starting point position, the landmark position, and the dead reckoning position, calculate the sum of the heading angle alignment error and the heading angle installation error of the inertial navigation module; The heading angle alignment error of the inertial navigation module is obtained by subtracting the sum from the heading angle installation error.
5. The calibration method for the inertial navigation and laser velocimeter combined navigation and positioning system according to claim 4, characterized in that, The formula for calculating the sum of the heading angle alignment error and the heading angle installation error of the inertial navigation module is as follows: ； In the formula, For heading angle alignment error, For the heading angle installation error, To determine the latitude value of the starting point, To determine the longitude value of the starting point, This represents the latitude value of the landmark location. This represents the longitude value of the landmark location. The latitude value for dead reckoning. The longitude value used to calculate the position for dead reckoning.
6. The calibration method for the inertial navigation and laser velocimeter combined navigation and positioning system according to claim 1, characterized in that, The formula for calculating the pitch angle installation error of the inertial navigation module is: ； In the formula, This is the difference between the altitude of the landmark location and the altitude of the dead reckoning location. The difference between the height of the starting point and the height of the landmark point; This refers to the scale factor error of the speedometer. To account for installation error in pitch angle, This refers to the distance traveled by the moving target during the calibration process.
7. A positioning method for a combined inertial navigation and laser velocimeter navigation and positioning system, characterized in that, The positioning method of the inertial navigation and laser velocimeter combined navigation and positioning system includes: Obtain the error parameters of the inertial navigation and laser velocimeter integrated navigation and positioning system; the error parameters include the scale factor error of the laser velocimeter, and the pitch angle installation error, heading angle installation error, and heading angle alignment error of the inertial navigation module; the error parameters are determined based on the calibration method of the inertial navigation and laser velocimeter integrated navigation and positioning system according to any one of claims 1-6; Obtain the dead reckoning position of the moving target in the navigation coordinate system at the previous moment; The system acquires the current velocity of the moving target measured by the laser velocimeter, the attitude transformation matrix of the inertial navigation module, and the pitch and yaw angles output by the inertial navigation module. Based on the dead reckoning position of the moving target in the navigation coordinate system at the previous moment, the error parameters, the velocity of the moving target measured by the laser velocimeter at the current moment, the attitude transformation matrix of the inertial navigation module, and the pitch and heading angles output by the inertial navigation module, the dead reckoning position of the moving target in the navigation coordinate system at the current moment is calculated.
8. A computer device, comprising: A memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that the processor executes the computer program to implement the calibration method of the inertial navigation and laser velocimeter integrated navigation and positioning system according to any one of claims 1-6 or the positioning method of the inertial navigation and laser velocimeter integrated navigation and positioning system according to claim 7.
9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by a processor, the computer program implements the calibration method of the inertial navigation and laser velocimeter combined navigation and positioning system as described in any one of claims 1-6, or the positioning method of the inertial navigation and laser velocimeter combined navigation and positioning system as described in claim 7.
10. A computer program product, comprising a computer program, characterized in that, When executed by a processor, the computer program implements the calibration method of the inertial navigation and laser velocimeter combined navigation and positioning system as described in any one of claims 1-6, or the positioning method of the inertial navigation and laser velocimeter combined navigation and positioning system as described in claim 7.