Track high-precision detection method and system based on inertial navigation

Abstract

The invention discloses a high-precision track detection method and system based on inertial navigation, and relates to the technical field of track smoothness detection. According to the method, first error data and second error data are obtained by combining a second speed and a second coordinate of a navigation positioning device and a third speed and a third displacement of a mileage measurement device, a first calibration distance and a second calibration distance are calculated, and a corresponding calibration state is entered at the calibration distance; attitude compensation data and equipment compensation data are updated in a calibration state, so that error accumulation caused by long-time continuous detection is avoided. According to the method, the instantaneous quantity of the compensation data at each moment is predicted according to the first smooth model and the second smooth model, the instantaneous angular velocity is adjusted by the instantaneous quantity, and the actual values of the velocity and the coordinates are generated, so that the measurement precision of the track geometric data is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of track smoothness detection, and in particular to a high-precision track detection method and system based on inertial navigation. Background Art

[0002] Inertial navigation technology can be used to detect the smoothness of the track. For example, the high-speed magnetic levitation track unevenness detection system and method based on inertial navigation disclosed in Chinese patent announcement number CN115597535B measures the motion parameters of the mobile measurement vehicle through an INS / GNSS combined navigation module, an odometer, and an inclination sensor, obtains track geometry data in combination with the initialization parameters of the inertial device, and then determines the smoothness of the track based on the track geometry data. Since the measurement error of the inertial device continues to diverge, the patent avoids the accumulation of errors in parameters such as speed, attitude, and position by establishing an inertial sensor error equation. During the operation of the inertial device, the initialization parameters of the inertial device gradually drift. The longer the track, the greater the fluctuation of the initialization parameters, and the greater the impact on the accuracy. It is necessary to reinitialize the inertial device and compensate the measurement results of the inertial device according to the current error situation. Therefore, the prior art needs further improvement. Summary of the invention

[0003] In order to solve the defects of the above-mentioned prior art, the present invention proposes a high-precision orbit detection method and system based on inertial navigation. The method determines the calibration state according to error data, updates the compensation data of the inertial device in the calibration state, and improves the measurement accuracy of the orbit geometry data.

[0004] The technical solution of the present invention is achieved in this way: A high-precision track detection method based on inertial navigation comprises the following steps: Step 1: The mobile device is arranged at the starting point of the track to be measured, the reference coordinates of the mobile device are collected, the mobile device enters the first calibration state, and the attitude compensation data of the inertial measurement device is generated. The mobile device enters the second calibration state and generates the equipment compensation data of the inertial measurement device; Step 2: The mobile device enters a moving state, the inertial measurement device collects instantaneous angular velocity and instantaneous acceleration, the navigation and positioning device collects the second velocity and the second coordinate, and the mileage measurement device collects the third velocity and the third displacement; Step 3: Obtain a first speed of the mobile device according to the posture compensation data, the device compensation data, and multiple sets of instantaneous angular velocities and instantaneous accelerations, and generate a first coordinate according to the first speed and the reference coordinate; Step 4: Obtain first error data according to the first coordinate and the second coordinate, and obtain second error data according to the first speed and the third speed; Step 5: Calculate a first calibration distance based on the first error data, and calculate a second calibration distance based on the second error data; Step 6: If the mobile device travels to the first calibration distance, the mobile device enters the first calibration state and updates the attitude compensation data and the reference coordinates; Step 7: If the mobile device travels to the second calibration distance, the mobile device enters the second calibration state and updates the device compensation data and the reference coordinates; Step 8: If the mobile device reaches the final measurement point, go to step 9, otherwise return to step 2; Step 9: Generate a first smoothing model according to the multiple sets of posture compensation data, and generate a second smoothing model according to the multiple sets of device compensation data; Step 10: Calculate the actual velocity through the first smoothing model, the second smoothing model, the instantaneous angular velocity, and the instantaneous acceleration, and then generate the geometric data of the track to be measured.

[0005] In the present invention, in step 1, in the first calibration state, the angular velocity A1 and acceleration B1 of the inertial measurement device when the mobile device is stationary are collected, the reference transformation matrix E1 of the inertial coordinate system to the reference coordinate system is calculated, and the attitude compensation data D1 is generated.

[0006] In the present invention, in step 1, in the second calibration state, multiple excitation angular velocities are sequentially applied to the inertial measurement device, the angular velocity A2 of the inertial measurement device is collected, and the device compensation data D2 is generated.

[0007] In the present invention, in step 3, an instantaneous transformation matrix E2 is generated according to the posture compensation data D1, the equipment compensation data D2 and multiple groups of instantaneous angular velocities A3, the first speed V1 of the mobile device is obtained according to the instantaneous acceleration B3 and the instantaneous transformation matrix E2, and the first coordinate S1 is generated according to the first speed V1 and the reference coordinate S0.

[0008] In the present invention, in step 4, the first error data Δ1 is the matrix norm of the coordinate difference, Δ1=‖S1- S2‖, the second error data Δ2 is the absolute value of the speed difference, Δ2=|‖V1‖- V3|, S2 is the second displacement, and V3 is the third speed.

[0009] In the present invention, in step 5, the first calibration distance L1=ηL0 2 / (Δ1+c)+‖S'-S0‖, the second calibration distance L2=ηL0 2 / (Δ2T0+c)+‖S'-S0‖, S' is the current coordinate, η is the accuracy control parameter, L0 is the distance between the starting point and the end point, T0 is the benchmark measurement time, and c is a constant.

[0010] In the present invention, if the distance between the second coordinate of the navigation positioning device and the reference coordinate is greater than or equal to the first calibration distance, the mobile device travels to the first calibration distance; if the third displacement of the mileage measurement device is greater than or equal to the second calibration distance, the mobile device travels to the second calibration distance.

[0011] In the present invention, in step 9, the trajectory length L3 of the mobile device when it is in the first calibration state is calculated, and a first smoothing model D3=g1(L3) of multiple trajectory lengths L3 and attitude compensation data is fitted, where D3 is the instantaneous value of the attitude compensation data. The average angular velocity A4 of the mobile device in the adjacent second calibration state is calculated, and a second smoothing model D4=g2(A4) of multiple average angular velocities A4 and equipment compensation data is fitted, where D4 is the instantaneous value of the equipment compensation data.

[0012] In the present invention, in step 10, the posture compensation data D1 is corrected according to the first smooth model, the equipment compensation data D2 is corrected according to the second smooth model, the actual angular velocity A5 is calculated with the corrected equipment compensation data, the actual transformation matrix E3 is calculated with the corrected posture compensation data, the actual speed V4 is obtained according to the actual transformation matrix E3 and the instantaneous acceleration B3, the actual coordinates S4 of the mobile device are obtained according to the actual speed V4, and then the geometric data of the track to be measured is obtained from the actual coordinates S4.

[0013] A track detection system for realizing the high-precision track detection method based on inertial navigation, comprising: a mobile device, an inertial measurement device, a navigation and positioning device, a mileage measurement device, an equipment calibration device, a coordinate solving device, a calibration control device, a data analysis device and a data processing device, The mobile device has a rigid carrier and at least two sets of mobile wheels, the inertial measurement device and the navigation and positioning device are installed on the rigid carrier, the mileage measurement device is installed on the mobile wheels, and the mobile wheels are arranged on the track to be measured; The equipment calibration device is used to update the reference coordinates, attitude compensation data, and equipment compensation data; The inertial measurement device includes a laser gyro component and an acceleration measurement component, the laser gyro component is used to collect instantaneous angular velocity, and the acceleration measurement component is used to collect instantaneous acceleration; The coordinate solving device is used to calculate the first speed and the first coordinate of the moving device; The navigation and positioning device is used to collect the second speed and the second coordinate of the mobile device; The mileage measuring device is used to collect a third speed and a third displacement of the mobile device; The calibration control device is used to calculate the first calibration distance and the second calibration distance and control the mobile device to enter the first calibration state or the second calibration state; The data analysis device is used to generate a first smoothing model and a second smoothing model; The data processing device is used to generate geometric data of the track to be measured.

[0014] The high-precision orbit detection method and system based on inertial navigation of the present invention has the following beneficial effects: the present invention combines the data update of the navigation positioning device and the mileage measurement device, determines the error accumulation of the inertial navigation and controls the mobile device to enter the first calibration state and the second calibration state, and updates the reference coordinates and corresponding compensation data in the first calibration state and the second calibration state. Furthermore, the present invention predicts the instantaneous amount of compensation data at each moment according to the type of compensation data, adjusts the instantaneous angular velocity by the instantaneous amount and generates the actual value of the velocity and coordinates, thereby improving the measurement accuracy of the orbit geometry data. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 is a schematic diagram of a horizontal deformable track; Figure 2 is a schematic diagram of a vertically deformable track; Figure 3 It is a flow chart of the high-precision orbit detection method based on inertial navigation of the present invention; Figure 4 It is a schematic diagram of the distribution of a plurality of first calibration states and a second calibration state on the track to be tested according to the present invention; Figure 5 A schematic diagram of updating the posture compensation data, the equipment compensation data, and the reference coordinates of the present invention; Figure 6 A schematic diagram of a first smoothing model preferred in the present invention; Figure 7 A schematic diagram of a second smoothing model preferred in the present invention; Figure 8 Schematic diagram of the inertial coordinate system and the reference coordinate system at different sampling times of the present invention; Fig. 9 A schematic diagram of providing an excitation angular velocity to an inertial measurement device according to the present invention; Fig.10 A schematic diagram of generating geometric data of a track to be measured for the present invention; Fig.11 A block diagram of a track detection system for realizing the high-precision track detection method based on inertial navigation according to the present invention; Fig.12 Schematic diagram of the installation position of the inertial measurement device of the present invention.

[0016] Reference numerals in the drawings: track to be measured 100 , rigid carrier 210 , moving wheel 220 , inertial measurement device 300 , position vector 400 , excitation axis 500 . DETAILED DESCRIPTION

[0017] In order to more clearly understand the purpose, technical solutions and advantages of the present application, the present application is described and illustrated below in conjunction with the accompanying drawings and embodiments.

[0018] Due to external interference such as traffic and weather, the track may undergo irreversible deformation, resulting in changes in geometric data. Figure 1 and Figure 2 As shown, the three-dimensional deformation of the track includes distortion in the horizontal direction and collapse in the vertical direction. It is known that the prior art can calculate the three-dimensional posture data of the detection vehicle through an inertial navigation device, calculate the geometric data of the track based on the three-dimensional posture data, and thus estimate the three-dimensional deformation of the track. However, the inertial navigation device needs to continuously iterate and update the initial data of the starting point to obtain the real-time three-dimensional posture data of the detection vehicle. The error of the previous iteration process will continue to accumulate to the next iteration, thereby affecting the detection accuracy. Embodiment 1

[0019] The present invention determines the calibration state according to the error data, updates the compensation data of the inertial measurement device in the calibration state, and improves the measurement accuracy of the track geometry data. The attitude compensation data is used to compensate for the initial attitude error, and the equipment compensation data is used to compensate for the equipment output error. Figures 3 to 7 The high-precision orbit detection method based on inertial navigation of the present invention includes the following steps.

[0020] Step 1: The mobile device is arranged at the starting point of the track to be measured, the reference coordinates of the mobile device are collected, the mobile device enters the first calibration state, the attitude compensation data of the inertial measurement device is generated, and the mobile device enters the second calibration state to generate the equipment compensation data of the inertial measurement device. The initial reference coordinate S0 is the three-dimensional coordinate of the starting point. In the first calibration state, the mobile device is in a stationary state and the inertial measurement device is in an unexcited state. At this time, the measurement error of the inertial measurement device includes the initial attitude error. The angular velocity A1 and acceleration B1 of the inertial measurement device when the mobile device is stationary are collected, and the reference transformation matrix E1 of the inertial coordinate system to the reference coordinate system is calculated to generate attitude compensation data D1. In the second calibration state, the mobile device is in a stationary state and the inertial measurement device is in an excited state. At this time, the measurement error of the inertial measurement device includes the equipment output error. In the second calibration state, multiple excitation angular velocities are applied to the inertial measurement device in turn, the angular velocity A2 of the inertial measurement device is collected, and the equipment compensation data D2 is generated. Example 2 further details the method for generating compensation data.

[0021] Step 2: The mobile device enters the motion state, the inertial measurement device collects the instantaneous angular velocity and instantaneous acceleration, the navigation and positioning device collects the second velocity and the second coordinate, and the mileage measurement device collects the third velocity and the third displacement. In the motion state, the mobile device travels along the track to be measured, and the inertial measurement device is stimulated by the track to be measured. At this time, the measurement error of the inertial measurement device at each sampling moment includes the initial attitude error and the device output error. The measurement value of the navigation and positioning device is obtained, for example, through the global navigation satellite system or the Beidou satellite navigation system. The measurement value of the mileage measurement device is calculated, for example, by collecting the rotation speed of the moving wheel and the radius of the moving wheel through a magnetic encoder / optical encoder. Among them, the measurement value of the mileage measurement device belongs to one-dimensional data, that is, the third velocity and the third displacement are scalars.

[0022] Step 3: Obtain the first speed of the mobile device according to the posture compensation data, the equipment compensation data, and multiple sets of instantaneous angular velocities and instantaneous accelerations, and generate the first coordinate according to the first speed and the reference coordinates. Specifically, generate the instantaneous transformation matrix E2 according to the posture compensation data D1, the equipment compensation data D2, and multiple sets of instantaneous angular velocities A3. The instantaneous transformation matrix E2 is the transformation matrix that transforms the inertial coordinate system to the reference coordinate system at any sampling moment. According to the instantaneous acceleration B3 and the instantaneous transformation matrix E2, the acceleration B3' of the mobile device in the reference coordinate system is obtained, and the time integral of the acceleration B3' is the first speed V1, and the sum of the time integral of the first speed V1 and the reference coordinate S0 is the first coordinate S1. The solution method of the first coordinate is further described in Example 3.

[0023] Step 4: Obtain the first error data according to the first coordinate and the second coordinate, and obtain the second error data according to the first speed and the third speed. The navigation and positioning device can update the measurement value in real time, and the first error data can describe the accumulation speed of the initial attitude error. The initial error of the mileage measurement device has an accumulation characteristic, and the difference between the first speed and the third speed can describe the size of the device output error. The first coordinate and the second coordinate of the present invention are three-dimensional position data, and the first error data Δ1 is the matrix norm of the coordinate difference, Δ1=‖S1- S2‖, and S2 is the second displacement. The meaning of the first error data Δ1 is the Euclidean distance between the first coordinate and the second coordinate. The first speed of the present invention is three-dimensional position data. The second error data Δ2 is the absolute value of the speed difference, Δ2=|‖V1‖- V3|, and V3 is the third speed. Limited by the function of the mileage measurement device, the third speed is a one-dimensional scalar. The meaning of the second error data Δ2 is the absolute value of the difference between the modulus of the first speed and the third speed.

[0024] Step 5: Calculate a first calibration distance based on the first error data, and calculate a second calibration distance based on the second error data. First calibration distance L1=ηL0 2 / (Δ1+c)+‖S'-S0‖, the second calibration distance L2=ηL02 / (Δ2T0+c)+‖S'-S0‖. S' is the current coordinate, that is, the first coordinate at the current sampling moment. ‖S'-S0‖ is the trajectory length between the current coordinate and the reference coordinate. η is the accuracy control parameter, which is used to control the sensitivity of the calibration and is usually set to 10 4 Up to 10 6 . L0 is the distance between the starting point and the end point, T0 is the reference measurement time, usually 1 hour to 10 hours. c is a small constant to avoid the denominator being zero. The larger the first error, the greater the cumulative attitude error of the inertial measurement device, and the smaller the first calibration distance. The larger the second error, the greater the device output error of the inertial measurement device, and the smaller the second calibration distance.

[0025] Step 6: If the mobile device reaches the first calibration distance, the mobile device enters the first calibration state and updates the attitude compensation data and reference coordinates. Otherwise, continue to drive and enter step 7. Figure 5 In the first calibration state, the compensation data is updated after reaching the calibration distance. If the distance between the second coordinate of the navigation and positioning device and the reference coordinate is greater than or equal to the first calibration distance, the mobile device moves to the first calibration distance. After entering the first calibration state, the mobile device is stationary, the angular velocity and acceleration of the inertial measurement device are collected, and then the attitude compensation data is updated. At the same time, the second coordinate of the mobile device after it stops is used as the new reference coordinate.

[0026] Step 7: If the mobile device travels to the second calibration distance, the mobile device enters the second calibration state and updates the device compensation data and the reference coordinates. Otherwise, continue driving and enter step 8. The mileage measurement device cannot directly obtain three-dimensional data, but can obtain the third displacement based on the scalar of the third speed. Specifically, if the third displacement of the mileage measurement device is greater than or equal to the second calibration distance, the mobile device travels to the second calibration distance. After entering the second calibration state, the mobile device is stationary, and the excitation angular velocity is applied to the three axes of the inertial measurement device respectively, the angular velocity and acceleration of the inertial measurement device are collected, and the device compensation data is updated. At the same time, the second coordinate after the mobile device is stationary is used as the new reference coordinate.

[0027] Step 8: If the mobile device reaches the final measurement point, go to step 9, otherwise return to step 2. The first coordinate is measured in real time. If at least one coordinate value (matrix element value) of the first coordinate is greater than the corresponding coordinate value of the final measurement point, the mobile device reaches the final measurement point.

[0028] Step 9: Generate a first smooth model based on multiple sets of posture compensation data, and generate a second smooth model based on multiple sets of device compensation data. This embodiment calculates the trajectory length L3 and the corresponding posture compensation data each time the mobile device is in the first calibration state. The trajectory length L3 is equal to the distance between the mobile device and the starting point in the first calibration state. Fit multiple trajectory lengths L3 and posture compensation data to generate a first smooth model D3=g1(L3), where D3 is the instantaneous value of the posture compensation data and g1 is the model function. This embodiment fits the first smooth model into three sets of linear functions, and each element value of the instantaneous value D3 of the posture compensation data is a linear function of the trajectory length L3. Refer to Figure 6 ,As the trajectory length increases, the element value β0 of the attitude compensation data gradually accumulates.

[0029] This embodiment extracts multiple instantaneous angular velocities A3 when the mobile device is in an adjacent second calibration state, and calculates the average angular velocity A4. Each average angular velocity corresponds to the device compensation data collected in this interval. The larger the average angular velocity, the larger the device output error, and the larger the corresponding device compensation data. Fit multiple groups of average angular velocities A4 and device compensation data to generate a second smoothing model D4=g2(A4), where D4 is the instantaneous value of the device compensation data, and g2 is the model function. In the second smoothing model, each element value of the instantaneous value D4 of the device compensation data is a function of the corresponding element value of the average angular velocity A4. Refer to Figure 7 In this embodiment, the second smoothing model is fitted into three sets of quadratic functions. As the element value of the average angular velocity increases, the corresponding element value λ of the device compensation data increases. x Increase.

[0030] Step 10: Calculate the actual speed through the first smoothing model, the second smoothing model, the instantaneous angular velocity, and the instantaneous acceleration, and then generate the geometric data of the track to be measured. As described in Example 4, the corrected attitude compensation data is obtained according to the first smoothing model, and the equipment compensation data is corrected according to the second smoothing model. The actual angular velocity A5 is calculated with the corrected equipment compensation data, and the actual transformation matrix E3 is calculated with the corrected attitude compensation data. The actual speed V4 is obtained according to the actual transformation matrix E3 and the instantaneous acceleration B3, the actual coordinate S4 of the mobile device is obtained according to the actual speed V4, and then the geometric data of the track to be measured is obtained from the actual coordinate S4. The actual coordinate is solved by referring to the solution method of the first coordinate of Example 2. The geometric data of the track to be measured is parallel to the trajectory of the inertial measurement device. The position vector of the track to be measured and the inertial measurement device is subtracted from the actual coordinate S4 to obtain the geometric data of the track to be measured. The present invention optimizes the attitude compensation data and the equipment compensation data, replaces the instantaneous angular velocity A3 and the instantaneous transformation matrix E2 with the actual angular velocity A5 and the actual transformation matrix E3, and improves the measurement accuracy of the track geometry data. Embodiment 2

[0031] This embodiment further discloses the method of generating posture compensation data and device compensation data in step 1.

[0032] like Figure 8 , this embodiment first defines three coordinate systems, including the earth coordinate system O0X0Y0Z0, the reference coordinate system O1X1Y1Z1, and the inertial coordinate system O1X2Y2Z2. The earth coordinate system takes the center of the earth as the origin O0, and the intersection of the equator and the prime meridian in the equatorial plane is the positive direction of the X0 axis, and the positive direction of the Y0 axis is the north pole. The Z0 axis is determined according to the right-hand rule. The reference coordinate system takes the center of mass of the inertial measurement device as the origin O1, and the positive direction of the X1 axis is the east direction, and the positive direction of the Y1 axis is the north direction. The Z1 axis is determined according to the right-hand rule. When the inertial measurement device is fixed in the middle of the vehicle body, the origin of the inertial coordinate system is the same as that of the reference coordinate system, and the three groups of mutually perpendicular sensitive elements of the inertial measurement device are the positive directions of the X2, Y2, and Z2 axes. When the train is stationary, the inertial reference system is transformed to the reference coordinate system via the reference transformation matrix. When the train is moving, the inertial reference system is transformed to the reference coordinate system via the instantaneous transformation matrix.

[0033] In the first calibration state, the angular velocity A1 and acceleration B1 of the inertial measurement device are collected when the mobile device is stationary. In the inertial coordinate system O1X2Y2Z2, the angular velocity , element value ω 1x ,ω 1y ,ω 1z They are the three uniaxial angular velocity components. , element value a 1x 、a 1y 、a 1z They are the three uniaxial acceleration components respectively.

[0034] Since the mobile device is at rest, the angular velocity of the inertial measurement device in the reference coordinate system is , U is the latitude of the inertial measurement device. Since the length of the track to be measured is much smaller than the diameter of the earth, it is usually assumed that the latitude of the mobile device remains unchanged. ie is the angular velocity of the earth's rotation. The angular velocity A1 is caused by the earth's rotation, and there is a matrix equation: , E1 is the reference transformation matrix. The acceleration B1 is caused by the earth's gravity, and the acceleration in the reference coordinate system is , -g is the acceleration due to gravity. There is a matrix equation: .

[0035] By combining the above matrix equations, we can find the reference transformation matrix E1. E1 is a three-row and three-column matrix. , and then according to α0=arctan(E 31 / E 32 ), β0=arccos(E33 ),γ0=-arctan(E 13 / E 23 ) Determine the initial pitch angle α0, roll angle β0, and heading angle γ0. The attitude compensation data D1 is the initial three-axis attitude matrix, that is, The inertial coordinate system is rotated along the Z axis by a heading angle γ0, then along the Y axis by a roll angle β0, and then along the X axis by a pitch angle α0 to obtain the reference coordinate system.

[0036] In the second calibration state, multiple excitation angular velocities are applied to the inertial measurement device in sequence, which are , , . Reference Fig. 9 The three sets of excitation shafts 500 drive the inertial measurement device to rotate at a constant speed in three directions respectively, and collect the output values ​​ω of the inertial measurement device corresponding to the three uniaxial acceleration components in turn. 2x ,ω 2y ,ω 2z The device compensation data D2 is the deviation between the excitation acceleration and the output value. The element value λ of the device compensation data in the X1 direction is x =ω0 / ω 2x , the element value λ in the Y1 direction y =ω0 / ω 2y , the element value λ in the Z1 direction z =ω0 / ω 2z . . Embodiment 3

[0037] This embodiment further discloses a method for calculating the first coordinate in step 3. The core of calculating the first coordinate is to calculate the instantaneous angular velocity and instantaneous acceleration in the inertial coordinate system O1X2Y2Z2 to the reference coordinate system O1X1Y1Z1.

[0038] First, quaternions q0, q1, q2, and q3 are generated based on the attitude compensation data D1. q0=cos(α0 / 2)cos(β0 / 2)cos(γ0 / 2)+sin(α0 / 2)sin(β0 / 2)sin(γ0 / 2), q0=sin(α0 / 2)cos(β0 / 2)cos(γ0 / 2)-cos(α0 / 2)sin(β0 / 2)sin(γ0 / 2), q2=cos(α0 / 2)sin(β0 / 2)cos(γ0 / 2)+sin(α0 / 2)cos(β0 / 2)sin(γ0 / 2), q3=cos(α0 / 2)cos(β0 / 2)sin(γ0 / 2)-sin(α0 / 2)sin(β0 / 2)cos(γ0 / 2). instantaneous transformation matrix .

[0039] Collect the instantaneous angular velocity A3, ,ω 3x ,ω 3y ,ω 3z They are the output values ​​of the three uniaxial acceleration components during the measurement process. The instantaneous angular velocity measurement value is calculated by the equipment compensation data D2 and multiple sets of instantaneous angular velocity A3. The instantaneous angular velocity measurement value is . Update quaternions q0', q1', q2', q3', , regenerate the instantaneous transformation matrix E2 according to the updated quaternion. The instantaneous acceleration in the inertial coordinate system, Acceleration in the reference coordinate system .

[0040] Speed ​​along the X1 axis , the speed along the Y1 axis , the speed along the Z1 axis . First Speed . Coordinates along the X1 axis , the coordinate along the Y1 axis , coordinates along the Z1 axis . Reference coordinates , the first coordinate . t is the duration of the sampling moment.

[0041] Continue to collect the instantaneous angular velocity A3 at the next sampling moment, and update the instantaneous transformation matrix E2, the first velocity V1, and the first coordinate S1. Figure 8 , every time t passes, the relationship between the inertial coordinate system where the instantaneous angular velocity is located and the reference coordinate system changes. The instantaneous transformation matrix E2 changes, and it is necessary to iteratively update the quaternion based on the current instantaneous transformation matrix and continue to solve the first velocity and the first coordinate. Embodiment 4

[0042] This embodiment further discloses a method of generating an actual transformation matrix according to the first smoothing model and generating an actual angular velocity according to the second smoothing model in the present invention.

[0043] Affected by the environmental magnetic field, dimension, etc., the inertial measurement device has an initial attitude compensation data (three-axis attitude matrix). In the subsequent data solution process, the attitude compensation data is used as the starting point to iteratively construct the transformation matrix. The error of the attitude compensation data will gradually accumulate. If the error is too large, the deviation of the first displacement measurement value after integration is large. The present invention optimizes the actual transformation matrix obtained in each first calibration state according to the first smoothing model. Specifically, the trajectory length L3 and the attitude compensation data when the mobile device is in each first calibration state are calculated. The trajectory length is used as the horizontal axis, and each element value of the attitude compensation data is used as the vertical axis to fit the first smoothing model. Figure 6, the vertical axis is b0, and the horizontal axis is the trajectory length L3. According to the first smoothing model D3=g1(L3), the trajectory length of the first calibration state is substituted to obtain the corresponding α0', β0', γ0' of the corrected attitude compensation data, and then the actual transformation matrix E3 is constructed.

[0044] Due to the limitations of equipment and instruments, the measured value will deviate from the true value, and the greater the excitation, the greater the offset. When the inertial measurement device is in high-speed vibration or movement, the difference between the first speed and the third speed increases significantly, and it is necessary to calibrate in each second calibration state and provide a corresponding compensation amount for the calibration value. Furthermore, the present invention predicts the compensation amount between the second calibration states to improve the measurement accuracy at any time. Specifically, each second calibration state and the previous second calibration state are regarded as an interval, and the average angular velocity A4 of the interval is used as the horizontal axis. The corresponding element value of the device compensation data collected at the end point of the interval (second calibration state) is used as the vertical axis, and the second smoothing model is fitted. Figure 7 , the vertical axis is the element value λ of the device compensation data in the X1 direction x , the horizontal axis is the angular velocity component of the average angular velocity A4 in the X1 direction. According to the second smoothing model D4=g2(A4), substitute the angular velocity component ω of any instantaneous angular velocity A3 3x ,ω 3y ,ω 3z , obtain the device compensation data after the instantaneous angular velocity correction, multiply the corrected device compensation data by the instantaneous angular velocity to obtain the actual angular velocity, the actual angular velocity ,ω 3x ',ω 3y ',ω 3z ', are the angular velocity components in three directions respectively.

[0045] Referring to the solution method of the third embodiment, the actual acceleration in the reference coordinate system is obtained by the actual transformation matrix E3 and the instantaneous acceleration B3, and then the actual velocity V4 is obtained by integration. The actual coordinate S4 of the mobile device is obtained according to the actual velocity V4. The position vector of the track and the inertial measurement device is subtracted from the actual coordinate S4 to obtain the geometric data of the track to be measured. Then the actual transformation matrix E3 is updated according to the actual angular velocity A5 to complete the iterative calculation. The data processing process is as follows: Fig.10 shown. Embodiment 5

[0046] like Fig.11 and Fig.12As shown, a track detection system for realizing the high-precision track detection method based on inertial navigation of the present invention includes: a mobile device, an inertial measurement device, a navigation and positioning device, a mileage measurement device, an equipment calibration device, a coordinate solution device, a calibration control device, a data analysis device and a data processing device. The mobile device has a rigid carrier 210 and at least two sets of mobile wheels 220, the inertial measurement device 300, the navigation and positioning device, the equipment calibration device, the coordinate solution device and the calibration control device are installed on the rigid carrier 210, the mileage measurement device is installed on the mobile wheel 220, and the mobile wheel 220 is arranged on the track 100 to be measured. After the arrangement is completed, the distance between the track to be measured and the inertial measurement device can be collected, and the position vector 400 of the track to be measured 100 and the inertial measurement device 300 can be initialized. Due to the setting of the rigid carrier 210, the position vector 400 remains unchanged, and the shape change of the track 100 to be measured is reflected as the coordinate change of the inertial measurement device 300.

[0047] The equipment calibration device is used to update the reference coordinates, attitude compensation data, and equipment compensation data. The equipment calibration device, for example, includes three sets of excitation axes with drivers, which are used to provide excitation angular velocity ω0. The inertial measurement device includes a laser gyro assembly and an acceleration measurement assembly, the laser gyro assembly is used to collect instantaneous angular velocity, and the acceleration measurement assembly is used to collect instantaneous acceleration. The navigation and positioning device is used to collect the second speed and second coordinates of the mobile device. The mileage measurement device is used to collect the third speed and third displacement of the mobile device. The coordinate solver is used to calculate the first speed and first coordinate of the mobile device. The calibration control device is used to calculate the first calibration distance and the second calibration distance and control the mobile device to enter the first calibration state or the second calibration state. The data analysis device is used to generate a first smoothing model and a second smoothing model. The data processing device is used to generate geometric data of the track to be measured.

[0048] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A high-precision orbit detection method based on inertial navigation, characterized in that: The following steps are involved: Step 1: The mobile device is arranged at the starting point of the track to be measured, the reference coordinates of the mobile device are collected, the mobile device enters the first calibration state, and the attitude compensation data of the inertial measurement device is generated. The mobile device enters the second calibration state and generates the equipment compensation data of the inertial measurement device; Step 2: The mobile device enters a moving state, the inertial measurement device collects instantaneous angular velocity and instantaneous acceleration, the navigation and positioning device collects the second velocity and the second coordinate, and the mileage measurement device collects the third velocity and the third displacement; Step 3: Obtain a first speed of the mobile device according to the posture compensation data, the device compensation data, and multiple sets of instantaneous angular velocities and instantaneous accelerations, and generate a first coordinate according to the first speed and the reference coordinate; Step 4: Obtain first error data according to the first coordinate and the second coordinate, and obtain second error data according to the first speed and the third speed; Step 5: Calculate a first calibration distance based on the first error data, and calculate a second calibration distance based on the second error data; Step 6: If the mobile device travels to the first calibration distance, the mobile device enters the first calibration state and updates the attitude compensation data and the reference coordinates; Step 7: If the mobile device travels to the second calibration distance, the mobile device enters the second calibration state and updates the device compensation data and the reference coordinates; Step 8: If the mobile device reaches the final measurement point, go to step 9, otherwise return to step 2; Step 9: Generate a first smoothing model according to the multiple sets of posture compensation data, and generate a second smoothing model according to the multiple sets of device compensation data; Step 10: Calculate the actual velocity through the first smoothing model, the second smoothing model, the instantaneous angular velocity, and the instantaneous acceleration, and then generate the geometric data of the track to be measured.

2. The high-precision track detection method based on inertial navigation according to claim 1 is characterized in that: In step 1, in a first calibration state, the angular velocity A1 and acceleration B1 of the inertial measurement device when the mobile device is stationary are collected, a reference transformation matrix E1 of the inertial coordinate system to the reference coordinate system is calculated, and attitude compensation data D1 is generated.

3. The high-precision track detection method based on inertial navigation according to claim 2 is characterized in that: In step 1, in a second calibration state, multiple excitation angular velocities are sequentially applied to the inertial measurement device, the angular velocity A2 of the inertial measurement device is collected, and device compensation data D2 is generated.

4. The high-precision orbit detection method based on inertial navigation according to claim 3 is characterized in that: In step 3, an instantaneous transformation matrix E2 is generated based on the posture compensation data D1, the equipment compensation data D2 and multiple sets of instantaneous angular velocities A3, the first speed V1 of the mobile device is obtained based on the instantaneous acceleration B3 and the instantaneous transformation matrix E2, and the first coordinate S1 is generated based on the first speed V1 and the reference coordinate S0.

5. The high-precision orbit detection method based on inertial navigation according to claim 4 is characterized in that: In step 4, the first error data Δ1 is the matrix norm of the coordinate difference, Δ1=‖S1- S2‖, the second error data Δ2 is the absolute value of the speed difference, Δ2=|‖V1‖- V3|, S2 is the second displacement, and V3 is the third speed.

6. The high-precision track detection method based on inertial navigation according to claim 5 is characterized in that: In step 5, the first calibration distance L1=ηL0 2 / (Δ1+c)+‖S'-S0‖, the second calibration distance L2=ηL0 2 / (Δ2T0+c)+‖S'-S0‖, S' is the current coordinate, η is the accuracy control parameter, L0 is the distance between the starting point and the end point, T0 is the benchmark measurement time, and c is a constant.

7. The high-precision orbit detection method based on inertial navigation according to claim 1 is characterized in that: If the distance between the second coordinate of the navigation and positioning device and the reference coordinate is greater than or equal to the first calibration distance, the mobile device travels to the first calibration distance; if the third displacement of the mileage measurement device is greater than or equal to the second calibration distance, the mobile device travels to the second calibration distance.

8. The high-precision orbit detection method based on inertial navigation according to claim 4 is characterized in that: In step 9, the trajectory length L3 of the mobile device when it is in the first calibration state is calculated, and a first smoothing model D3=g1(L3) of multiple trajectory lengths L3 and attitude compensation data is fitted, where D3 is the instantaneous value of the attitude compensation data. The average angular velocity A4 of the mobile device in the adjacent second calibration state is calculated, and a second smoothing model D4=g2(A4) of multiple average angular velocities A4 and equipment compensation data is fitted, where D4 is the instantaneous value of the equipment compensation data.

9. The high-precision orbit detection method based on inertial navigation according to claim 4 is characterized in that: In step 10, the attitude compensation data D1 is corrected according to the first smooth model, the device compensation data D2 is corrected according to the second smooth model, the actual angular velocity A5 is calculated using the corrected device compensation data, the actual transformation matrix E3 is calculated using the corrected attitude compensation data, the actual velocity V4 is obtained based on the actual transformation matrix E3 and the instantaneous acceleration B3, the actual coordinates S4 of the mobile device are obtained based on the actual velocity V4, and then the geometric data of the track to be measured is obtained from the actual coordinates S4.

10. A track detection system for implementing the high-precision track detection method based on inertial navigation as claimed in claim 1, characterized in that: include: Mobile device, inertial measurement device, navigation and positioning device, mileage measurement device, equipment calibration device, coordinate solving device, calibration control device, data analysis device and data processing device, The mobile device has a rigid carrier and at least two sets of mobile wheels, the inertial measurement device and the navigation and positioning device are installed on the rigid carrier, the mileage measurement device is installed on the mobile wheels, and the mobile wheels are arranged on the track to be measured; The equipment calibration device is used to update the reference coordinates, attitude compensation data, and equipment compensation data; The inertial measurement device includes a laser gyro component and an acceleration measurement component, the laser gyro component is used to collect instantaneous angular velocity, and the acceleration measurement component is used to collect instantaneous acceleration; The coordinate solving device is used to calculate the first speed and the first coordinate of the moving device; The navigation and positioning device is used to collect the second speed and the second coordinate of the mobile device; The mileage measuring device is used to collect a third speed and a third displacement of the mobile device; The calibration control device is used to calculate the first calibration distance and the second calibration distance and control the mobile device to enter the first calibration state or the second calibration state; The data analysis device is used to generate a first smoothing model and a second smoothing model; The data processing device is used to generate geometric data of the track to be measured.

Images