An optimization method for INS and GPS combined navigation of unmanned surface vehicle

Abstract

The application discloses an optimization method for INS and GPS combined navigation of an unmanned ship, uses a double-propeller unmanned ship power model obtained by using the relationship between power signals and thrust of the unmanned ship to improve navigation precision of an inertial navigation system, and thus improves the INS / GPS combined navigation precision of the unmanned ship. When a global positioning system signal is available, a particle filter algorithm is used as a data fusion algorithm of the INS / GPS combined navigation system. In the case that the global positioning system signal is disconnected or weak, a power model of the double-propeller unmanned ship is used as an event-triggered switch to perform effective acceleration integration, so that the inertial navigation system of the unmanned ship can keep high-precision positioning. The method effectively solves the problem that the acceleration error measured by an economical inertial navigation system is large due to the influence of an external environment when the unmanned ship sails, and thus the positioning precision of the INS navigation system is low and the effective time is short.

Description

Technical Field

[0001] This invention relates to the field of unmanned surface vessel (USV) integrated navigation technology, and in particular to an optimization method for USV integrated INS and GPS navigation. Background Technology

[0002] The navigation system is one of the most important components of an unmanned surface vessel (USV) system, and it usually determines the accuracy and effectiveness of USV operations.

[0003] A combined navigation system consisting of the Global Positioning System (GPS) and the Inertial Navigation System (INS) can not only overcome the shortcomings of each system existing alone, but also give full play to the advantages of each system, improving the accuracy and reliability of the system, and is therefore more commonly used.

[0004] However, considering the influence of external factors such as wind, waves, and ocean currents, as well as internal factors such as IMU performance during actual navigation, INS navigation accuracy is low and its effective time is short. When GPS signal is interrupted or weak, the accuracy of INS / GPS combined navigation of unmanned surface vessels is relatively low and the navigation error is large. Summary of the Invention

[0005] Purpose of the invention: To address the above-mentioned problems, the purpose of this invention is to provide an optimized method for the integrated navigation of INS and GPS for unmanned surface vessels (USVs), solving the problems of low positioning accuracy and short effective time of USV INS navigation systems, and improving the positioning accuracy of integrated INS and GPS navigation.

[0006] Technical solution: An optimized method for unmanned surface vessel (USV) INS and GPS integrated navigation, comprising the following steps:

[0007] S1. Obtain the starting position information of the unmanned surface vessel. The inertial navigation data is processed in the carrier coordinate system b through attitude calculation and attitude coordinate transformation to obtain the attitude of the unmanned surface vessel in the navigation coordinate system n.

[0008] S2. By introducing a dynamic model and using event triggering, determine whether the unmanned surface vessel needs to perform effective acceleration integration:

[0009] If an effective acceleration integration is required, the y-axis of the carrier coordinate system b should be ignored. b axis and z b Integrating the inertial accelerometer data corresponding to the x-axis only for x b The actual heading speed of the unmanned surface vessel is obtained by integrating the inertial accelerometer data corresponding to the axis. The velocity in the navigation coordinate system n is obtained after coordinate rotation.

[0010] If there is no need to integrate the effective acceleration, the velocity in the n-frame is the same as the velocity at the previous moment.

[0011] S3. Integrate the northward velocity, eastward velocity, and skyward velocity in the n-system respectively to obtain the INS position information of the unmanned surface vessel;

[0012] S4. Check if the GPS signal is interrupted or weak:

[0013] If the GPS signal is interrupted or weak, the location information obtained in step S3 will be output directly.

[0014] If the GPS signal is normal, the location information obtained in step S3 and the GPS location information are subjected to particle filtering processing.

[0015] S5. Obtain the latitude and longitude location information of the integrated navigation system.

[0016] Furthermore, in step S1, the starting position information of the unmanned surface vessel (USV) is an existing known point or a real-time measurement point of the measuring instrument. The INS uses this as the integration starting point for the position information, and the navigation coordinate system n is an East-North-Sky coordinate system. The attitude of the USV in the n-system is att=[ψθφ]. T Where ψ is the real-time heading angle, θ is the real-time pitch angle, and φ is the real-time roll angle, all of which are given in real time by inertial navigation measurement.

[0017] Furthermore, in step S2, the dynamic model of the unmanned surface vessel (USV) is the mathematical relationship between the USV's power control signal and the thrust generated by the propellers. For a dual-propeller USV, the dynamic model is as follows:

[0018] τ u =f(s) L )+f(s R );

[0019] Among them, S L and S R These are the control signals for the left and right thrusters, respectively. f is the relationship between the thrust F of a single thruster and the control signal, and τ... u It is the sum of the thrust of the left and right thrusters.

[0020] The dynamic model of the unmanned surface vessel (USV) was obtained through experimental data. Only real-time control signal data and real-time thrust data were needed; the dynamic model was then derived through data fitting. In the USV experiments, the inertial navigation accelerometer was greatly affected by the environment, leading to significant errors in the acceleration data. Consequently, the velocity information obtained after integration was inaccurate. Analysis of the experimental data showed that the τ generated by the USV's thrusters… uWhen there are significant changes, the acceleration data measured by inertial navigation is relatively accurate; this acceleration data is called effective acceleration data. Therefore, selecting the integrated effective acceleration data will yield more accurate velocity and position information.

[0021] Furthermore, by comparing the previous moment... and the current moment if If the threshold range [-ε, ε] is within which the acceleration integration stops; otherwise, the integration of the effective acceleration begins, where ε > 0.

[0022] Furthermore, in step S2, when the effective acceleration needs to be integrated, the direction of the carrier coordinate system b is right-forward-upward, and the attitude transformation matrix from the b system to the n system is:

[0023]

[0024] Therefore, V in the n-system n for:

[0025]

[0026] For effective speed V b The effective velocity V converted to the n-system by the attitude transformation matrix n The three velocity components are the effective velocities in the east, north, and sky directions under n.

[0027] Furthermore, in step S3, the integral equations for the longitude, latitude, and altitude of INS are:

[0028]

[0029]

[0030]

[0031] λ, L, and h represent longitude, latitude, and altitude, respectively; λ0, L0, and h0 represent the initial longitude, latitude, and altitude, respectively; R M R is the radius of the Earth's meridian. N This is the radius of the Earth's circumpolar orbit.

[0032] Ideally, in step S4, the particle filtering process is as follows:

[0033] (1) Establish the state transition equations for the motion of the unmanned surface vessel:

[0034]

[0035] in and These represent the current northward and eastward velocities in the n-system, respectively, and the velocity V of rand relative to GPS. GPS (t) and the estimated velocity of the system at the previous time step The relevant data is that the number of sampled particles is N. They are uniformly distributed in this interval, where parameters k1,k2>0;

[0036] (2) Calculate the weight(i) of each particle and determine the λ of each particle. t (i) and L t (i) If the distance between the latitude and longitude of the reference location is dis, then the weight weight(i) is expressed as:

[0037]

[0038] Weight normalization:

[0039]

[0040] weight'(i) represents the normalized weights, and sum_weight represents the accumulated weights. The latitude and longitude λ of the reference location... S ,L S for:

[0041]

[0042] Where k3 is the weight parameter, and k3∈[0,1].

[0043] (3) Estimate latitude and longitude values:

[0044]

[0045]

[0046] (4) Set a weight threshold weight_min. When at least 1 / ξ of the particles has a weight less than this threshold, adjust the range parameters k1 and k2 for resampling so that the weight of less than 1 / ξ of the particles is less than weight_min. Where ξ > 1; if the particles with weights less than weight_min are distributed on the left side of the range, then k1 is increased appropriately; otherwise, k2 is decreased appropriately.

[0047] Beneficial effects: Compared with the prior art, the advantages of the present invention are:

[0048] 1. Easy to implement in engineering, with low algorithm complexity and low computational load.

[0049] 2. It achieves low cost and is suitable for economical INS / GPS integrated navigation systems using inertial navigation and GPS modules.

[0050] 3. The mathematical relationship between the control signal and the thruster thrust is easily obtained from experimental data.

[0051] 4. The INS / GPS integrated navigation system has high accuracy and long effective time.

[0052] 5. Using the dynamic model of the dual-thrust unmanned surface vessel (USV) as an event-triggered switch to perform effective acceleration integration, the USV's INS maintains high-precision positioning. This effectively solves the problem of low positioning accuracy and short effective time of the USV INS navigation system composed of an economical inertial measurement unit and navigation algorithm. By improving the accuracy of INS navigation alone, the accuracy of the USV INS / GPS integrated navigation is improved. Attached Figure Description

[0053] Figure 1 This is a flowchart of the present invention;

[0054] Figure 2 This is a schematic diagram of the carrier coordinate system and the navigation coordinate system;

[0055] Figure 3 A schematic diagram of the event triggering structure for an unmanned surface vessel to perform effective acceleration integration through a dynamic model;

[0056] Figure 4 This is a schematic diagram of the structure of an unmanned surface vessel INS / GPS integrated navigation system based on particle filtering and dynamic model. Detailed Implementation

[0057] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0058] An optimized method for unmanned surface vessel (USV) INS and GPS integrated navigation, such as... Figure 1 As shown, it includes the following steps:

[0059] Step S1: Obtain relatively accurate starting position information of the unmanned surface vessel (USV). The inertial navigation data, after attitude calculation and attitude coordinate transformation in the carrier coordinate system b, yields the USV's attitude in the navigation coordinate system n, such as... Figure 2 As shown.

[0060] In this embodiment of the invention, the point can be a known point or measured by a high-precision measuring instrument. The INS uses this as the starting point for integrating the position information. The n-frame is the navigation coordinate system, which is East-North-Sky. The attitude of the unmanned surface vessel in the n-frame is att = [ψθφ]. T ψ is the real-time heading angle, θ is the real-time pitch angle, and φ is the real-time roll angle, all of which are given in real time by the inertial navigation system.

[0061] Step S2: Determine whether the unmanned surface vessel needs to perform effective acceleration integration by using an event-triggered dynamic model.

[0062] In practical implementation, the dynamic model of an unmanned surface vessel (USV) is the mathematical relationship between its power control signals and the thrust generated by the propellers. The dynamic model for a dual-propeller USV is as follows:

[0063] τ u =f(s) L )+f(s R );

[0064] Among them, S L and S R These are the control signals for the left and right thrusters, respectively. f is the relationship between the thrust F of a single thruster and the control signal, and τ... u This represents the sum of the thrust from the left and right thrusters. The dynamic model of the unmanned surface vessel (USV) is derived from lake survey experimental data. Only real-time control signal data and real-time thrust data are needed; the dynamic model is obtained through data fitting. However, in actual lake survey experiments, the inertial navigation accelerometer is greatly affected by the environment, leading to significant errors in the acceleration data. Consequently, the velocity information obtained through integration will be inaccurate. Analysis of the experimental data shows that the τ generated by the USV's thrusters... u When there are significant changes, the acceleration data measured by inertial navigation is relatively accurate; this acceleration data is called effective acceleration data. Therefore, selecting the integrated effective acceleration data will yield more accurate velocity and position information. Figure 3 As shown, by comparing the previous moment and the current moment if If the acceleration integration is within the threshold range [-ε, ε], then acceleration integration stops; otherwise, integration of the effective acceleration begins. Where ε > 0.

[0065] Step S3: If integration of effective acceleration is required, ignore y. b and z b The corresponding inertial accelerometer data integration is only for x. b The actual heading speed of the unmanned surface vessel is obtained by integrating the inertial accelerometer data corresponding to the axis. The corresponding velocity in the n-frame is obtained after coordinate rotation.

[0066] In practice, if step S2 determines "yes", the effective accelerations of INS in the north, east, and sky directions are integrated to obtain more accurate velocities in these directions; for example... Figure 1 As shown, the method proposed in this invention ignores the y-axis in the b-system. b and z bThe acceleration corresponding to the axis is only considered for the x-axis pointing towards the bow. b The acceleration along the axis is integrated over its effective acceleration, and the resulting effective velocity is then rotated to the n-frame to obtain the effective velocity in the n-frame. Here, the carrier coordinate system b is right-front-up. The attitude transformation matrix from the b-frame to the n-frame is:

[0067]

[0068] Therefore, V in the n-system n for:

[0069]

[0070] For effective speed V b The effective velocity V converted to the n-system by the attitude transformation matrix n The three velocity components are the effective velocities in the east, north, and sky directions under n.

[0071] Step S4: If integration of effective acceleration is not required, the velocity in the n-frame is the velocity corresponding to the previous moment;

[0072] In practice, if the result of step S2 is "no", proceed to step S4, and the current V t n For the previous moment

[0073] Step S5: Integrate the northward velocity, eastward velocity, and skyward velocity in the n-system respectively to obtain the position information of the unmanned surface vessel (INS).

[0074] In practice, the northward and eastward velocities are integrated to obtain the INS location information. The integral equations for the longitude, latitude, and altitude of the INS are:

[0075]

[0076]

[0077]

[0078] λ, L, and h represent longitude, latitude, and altitude, respectively; λ0, L0, and h0 represent the initial longitude, latitude, and altitude, respectively; R M R is the radius of the Earth's meridian. N This is the radius of the Earth's circumpolar orbit.

[0079] Step S6: Check if the GPS signal is interrupted.

[0080] Step S7: At this point, the location information output by the integrated navigation system is only provided by the INS.

[0081] Step S8: Perform particle filtering on the INS latitude, longitude, and velocity data after effective acceleration integration, together with the GPS latitude, longitude, and velocity data. The particle filtering process is as follows:

[0082] S801: The state transition equation for the motion of the unmanned surface vessel is established as follows:

[0083]

[0084] in, and These represent the current northward and eastward velocities in the n-system, respectively, and the velocity V of rand relative to GPS. GPS (t) and the estimated velocity of the system at the previous time step The relevant data is that the number of sampled particles is N. They are uniformly distributed over this interval. The parameters k1 and k2 are greater than 0.

[0085] S802: Calculation of the weight(i) for each particle, determining the λ of each particle. t (i) and L t If the distance between (i) and the reference location is dis, then the weight weight(i) is expressed as:

[0086]

[0087] Weight normalization:

[0088]

[0089] weight'(i) represents the normalized weights, and sum_weight represents the accumulated weights. The latitude and longitude λ of the reference location... S ,L S for:

[0090]

[0091] Where k3 is the weight parameter, and k3∈[0,1];

[0092] S803: Estimated latitude and longitude values:

[0093]

[0094]

[0095] S804: Set a weight threshold weight_min. When at least 1 / ξ of the particles has a weight less than this threshold, adjust the range parameters k1 and k2 for resampling so that the weights of fewer than 1 / ξ particles are less than weight_min. Where ξ > 1. If the particles with weights less than weight_min are distributed on the left side of the range, then k1 is appropriately increased; otherwise, k2 is appropriately decreased.

[0096] Step S9: The output of the location information for the integrated navigation has two cases, such as... Figure 4 As shown:

[0097] S901: When the GPS signal is interrupted or weak, the latitude and longitude position information of the INS is output as the latitude and longitude position information of the integrated navigation system.

[0098] S902: When the GPS signal is normal, the latitude and longitude position information of the INS and the position information of the GPS are processed by particle filtering to obtain the latitude and longitude position information of the INS / GPS integrated navigation system.

[0099] The above embodiments are merely examples, but the present invention is not limited to the above embodiments. Even if various changes are made to the present invention, if these changes fall within the scope of the claims of the present invention and their equivalents, they still fall within the protection scope of the present invention.

Claims

1. An optimized method for unmanned surface vessel (USV) INS and GPS integrated navigation, characterized in that... Includes the following steps: S1. Obtain the starting position information of the unmanned surface vessel. The inertial navigation data is processed in the carrier coordinate system b through attitude calculation and attitude coordinate transformation to obtain the attitude of the unmanned surface vessel in the navigation coordinate system n. S2. By introducing a dynamic model and using event triggering, determine whether the unmanned surface vessel needs to perform effective acceleration integration: If an effective acceleration integration is required, the y-axis of the carrier coordinate system b should be ignored. b axis and z b Integrating the inertial accelerometer data corresponding to the x-axis only for x b The actual heading speed of the unmanned surface vessel is obtained by integrating the inertial accelerometer data corresponding to the axis. The velocity in the navigation coordinate system n is obtained after coordinate rotation. ; If there is no need to integrate the effective acceleration, the velocity in the n-frame is the same as the velocity at the previous moment. S3. Integrate the northward velocity, eastward velocity, and skyward velocity in the n-system respectively to obtain the INS position information of the unmanned surface vessel; S4. Check if the GPS signal is interrupted or weak: If the GPS signal is interrupted or weak, the location information obtained in step S3 will be output directly. If the GPS signal is normal, the location information obtained in step S3 and the GPS location information are subjected to particle filtering processing. S5. Obtain the latitude and longitude location information of the integrated navigation system; In step S3, the integral equations for the longitude, latitude, and altitude of INS are: ； ； ； h represents longitude, latitude, and altitude, respectively. h0 and r represent the initial longitude, latitude, and altitude, respectively; R M R is the radius of the Earth's meridian. N The radius of the Earth's circumpolar orbit; In step S4, the particle filtering process is as follows: (1) Establish the state transition equations for the motion of the unmanned surface vessel: ； in and These represent the current northward and eastward velocities in the n-system, and the speeds of rand and GPS, respectively. Estimated velocity of the system at the previous time step The relevant number of sampled particles is N. They are uniformly distributed in this interval, where parameters k1,k2>0; (2) Calculate the weight(i) of each particle to determine the weight of each particle. and If the distance between the latitude and longitude of the reference location is dis, then the weight weight(i) is expressed as: ； Weight normalization: ； The weights are the normalized values, and sum_weight is the accumulated weight value; the latitude and longitude of the reference location are also present. ,L S for: ； Where k3 is the weight parameter, and ; (3) Estimate latitude and longitude values: ； ； (4) Set a weight threshold weight_min, when at least there are When the weight of a particle is less than the threshold, the value interval parameters k1 and k2 are adjusted for resampling, so that the weight is less than the threshold. The particle's weight is less than weight_min. Wherein... If particles with weights less than weight_min are distributed on the left side of the value range, then k1 should be increased appropriately; otherwise, k2 should be decreased appropriately.

2. The optimization method for unmanned surface vessel (USV) INS and GPS integrated navigation according to claim 1, characterized in that: In step S1, the starting position information of the unmanned surface vessel (USV) is an existing known point or a real-time measurement point of the measuring instrument. INS uses this as the integration starting point for the position information, the navigation coordinate system n is the northeast-to-sky coordinate system, and the attitude of the USV in the n-system is... ,in, For real-time heading angle, For real-time pitch angle, The roll angle is given in real time by inertial navigation measurement.

3. The optimization method for unmanned surface vessel (USV) INS and GPS integrated navigation according to claim 1, characterized in that: In step S2, the dynamic model of the unmanned surface vessel (USV) is the mathematical relationship between the USV's power control signal and the thrust generated by the thrusters. For a dual-thrust USV, the dynamic model is as follows: ； Among them, S L and S R These are the control signals for the left and right thrusters, respectively, and f is the relationship between the thrust F of a single thruster and the control signal. It is the sum of the thrust of the left and right thrusters.

4. The optimization method for unmanned surface vessel (USV) INS and GPS integrated navigation according to claim 3, characterized in that: By comparing the previous moment and the current moment The value, if Within the threshold range If the acceleration is within a certain range, the integral of acceleration stops; otherwise, the integral of effective acceleration begins, where... ; 5. An optimization method for unmanned surface vessel (USV) INS and GPS integrated navigation according to claim 2, characterized in that: In step S2, when effective acceleration integration is required, the direction of the carrier coordinate system b is right-front-up, and the attitude transformation matrix from the b system to the n system is: ； Therefore, V in the n-system n for: ； For effective speed V b The effective velocity V converted to the n-system by the attitude transformation matrix n The three velocity components are the effective velocities in the east, north, and sky directions under n.

Images