A dynamic heading measurement and calibration method for unmanned surface vessels
Through the GNSS dual-antenna system and linear fitting model, the abnormal positioning data is deleted, which solves the time accumulation error and environmental limitation problems of inertial navigation systems in unmanned surface ships, and realizes efficient heading calibration, which is suitable for a variety of environments and carriers.
Patent Information
- Application Number
- CN202210340016.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-03-31
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2042-03-31
AI Technical Summary
Among the existing dynamic heading measurement methods for unmanned surface ships, the inertial navigation system is affected by time accumulation error, the measurement distance of the optical total station is limited and cannot be used in harsh environments, the GNSS system is affected by external interference, and the Kalman filtering method It requires accurate mathematical models and noise parameters, which are difficult to effectively use in practical applications.
The GNSS dual-antenna system is adopted to delete abnormal positioning data that affects and does not affect linear fit through the linear fit model and the 3σ criterion to realize heading calibration of the inertial navigation system without the need for complex mathematical models and precise noise parameters.
It realizes dynamic heading measurement calibration at low cost and easy to implement unmanned surface ships, improves heading accuracy, has a wide range of application and simple operation, and reduces the impact of GNSS positioning abnormal data on the inertial navigation system.
Smart Images

Figure CN114754795B_ABST
Abstract
Description
Technical Field
[0001] The present invention mainly relates to the technical field of unmanned surface vessels, and in particular to a dynamic heading measurement and calibration method for unmanned surface vessels based on deletion of abnormal GNSS (Global Navigation Satellite System) positioning data. Background Art
[0002] Unmanned surface vessel (USV) is a surface vehicle with autonomous navigation capability and can carry out diversified operations by carrying advanced control systems, sensor systems, communication systems or weapon systems. Compared with traditional manned ships, it has the advantages of good maneuverability, high degree of automation, low cost of use, etc. It can perform tasks that are difficult or impossible for manned ships to complete in dangerous and harsh environments, and has broad application prospects in both military and civilian fields.
[0003] High-precision heading data is the premise and basis for unmanned surface ships to maneuver and perform tasks. At present, high-precision inertial navigation systems are usually used to provide heading information for unmanned surface ships. The inertial navigation system is mainly composed of gyroscopes and accelerometers. Affected by the cumulative errors of gyroscopes and accelerometers, the heading error accumulates or even diverges over time. Therefore, researching and designing high-efficiency and high-reliability heading calibration technology to ensure that the heading data of the inertial navigation system is not affected by the time-accumulated error has important theoretical significance and practical value for effectively improving the operating efficiency of unmanned surface ships in actual working environments and promoting the further development and application of surface unmanned ship technology.
[0004] To this end, practitioners Bian Hongwei, Ma Heng, Jin Zhihua, Zhu Tao and others proposed a heading calibration scheme using dual total stations in their published paper "Heading Calibration System Based on Optical Total Station". This scheme uses a four-point direction and distance measurement method for the carrier to achieve high-precision heading measurement. The shortcomings of this type of method are: the measurement range of the optical total station is limited. The current mainstream optical total station measurement distance is ≤2km, and optical measurement equipment is difficult to use in low visibility, severe climate, haze, rain and snow, etc., and cannot meet the needs of large-scale, all-weather, real-time measurement and calibration of moving carriers.
[0005] In their paper "Study on the Processing Method of Heading Alignment Data Based on Dual Antenna Direction Finding", practitioners Bai Yudong, Li Jingyan and others proposed the use of GNSS dual antenna direction finding method to check the alignment of ship heading under dynamic conditions. This type of radio-based direction finding system has the advantages of short measurement time, high measurement accuracy, and no influence from visibility factors in the environment. However, when the GNSS system is interfered by external factors such as ionosphere and multipath, it will cause abnormal positioning data to be measured. If the measured positioning data is not deleted, it is very likely to introduce a large deviation to the calibration of the inertial navigation system.
[0006] In addition, practitioners Yang Zhao, Wang Jianhua, Wu Yuping and others published a paper titled "Application of Kalman Filter in Dual-antenna Positioning and Direction Finding System of Unmanned Boat" in the journal "Computer Measurement and Control". They used the Kalman filter algorithm to filter the collected GPS positioning data for the positioning error problem of the dual-antenna GPS (Global Positioning System) positioning and direction finding system of unmanned surface boats. The proposed method achieved good direction finding effect in simulation experiments. The shortcomings of this type of method are: the prerequisite for using the Kalman filter method is that there must be an accurate mathematical model and accurate noise parameters. In practical applications, the mathematical model and noise parameters of the system are difficult to accurately construct and calculate, and the Kalman filter algorithm only filters based on the prior mathematical model parameters and noise parameters, rather than real-time measurement parameters. When the actual measurement results are greatly different from the prior matrix model parameters and noise statistical parameters, the calculation results will diverge and lose the filtering effect. Therefore, the use of Kalman filtering to filter GPS positioning data in simulation experiments is effective, but it is difficult to meet actual usage needs. Summary of the invention
[0007] The technical problem to be solved by the present invention is: in view of the problems existing in the prior art, the present invention provides a dynamic heading measurement and calibration method for unmanned surface ships which is low-cost, easy to implement, and can accurately delete abnormal positioning data.
[0008] In order to solve the above technical problems, the present invention adopts the following technical solutions:
[0009] A method for calibrating the dynamic heading measurement of an unmanned surface vessel, the steps comprising:
[0010] Step S1: Install GNSS dual antennas and use the timing function of the GNSS system to synchronize the time system of the unmanned surface ship;
[0011] Step S2: While the unmanned ship is executing the linear path, the heading data of the inertial navigation system and the positioning data at the positions of the GNSS dual antennas at the same time are collected;
[0012] Step S3: deleting abnormal positioning data that affects linear fitting;
[0013] Step S4: deleting abnormal positioning data that does not affect linear fitting;
[0014] Step S5: Utilize the GNSS dual antenna system to collect effective positioning data at the same time, calculate the heading at that time, and compare it with the heading data of the inertial navigation system at the same time to achieve the calibration of the heading of the inertial navigation system.
[0015] As a further improvement of the method of the present invention: in the step S1, two GNSS antennas are respectively installed on the bow and stern lines of the unmanned surface ship, the GNSS antenna installed in the bow direction is marked as antenna A, and the GNSS antenna installed in the stern direction is marked as antenna B, so as to obtain a GNSS dual-antenna heading measurement system; the time synchronization function of the GNSS system is used to synchronize the time unified system of the unmanned surface ship to ensure that all equipment on the unmanned surface ship operates on the same time reference.
[0016] As a further improvement of the method of the present invention: in the step S2, the unmanned surface vessel performs linear motion along the bow and stern line at a constant speed V, and uses the same sampling time and sampling frequency to collect the GNSS positioning data at the positions of antenna A and antenna B and the heading data of the inertial navigation system to obtain the heading data set H of the inertial navigation system = {h 1 ,h 2 ,…,h i ,…,h n}、GNSS antenna A positioning data set D A ={A 1 ,A 2 ,…,A i ,…,A n}, positioning data set D of GNSS system antenna B B = {B 1 ,B 2 ,…,B i ,…,B n}; where 1≤i≤n, n is a positive integer, h i is the i-th data member in the heading data set H, h i ={t i ,f i}, t i is the sampling time, f i t i Heading data collected at all times; A i To locate the dataset D A The i-th position data member in t iThe horizontal coordinate of antenna A in the geodetic coordinate system at this moment, t i The vertical coordinate data of antenna A at the moment; B i To locate the dataset D B The i-th member data in t i The horizontal coordinate of antenna B in the geodetic coordinate system at this moment, t i The vertical coordinate of antenna B at time.
[0017] As a further improvement of the method of the present invention: in the step S3, the collected GNSS dual-antenna positioning data is processed by a linear regression method to obtain the equation of the linear motion path of the unmanned surface ship. Due to the influence of external interference factors, there will inevitably be abnormal positioning data in the data collected by GNSS. The existence of abnormal positioning data will introduce unpredictable errors into the solution of the linear motion trajectory equation of the unmanned surface ship and the solution of the GNSS dual-antenna heading. The method of the present invention cleverly divides the abnormal positioning data into abnormal positioning data that affects the linear regression fitting and abnormal positioning data that does not affect the linear regression fitting according to the characteristics of the unmanned surface ship executing the linear motion path: the abnormal positioning data that affects the linear fitting refers to the GNSS positioning data that seriously deviates from the true value of the geographic coordinates in the direction perpendicular to the linear motion direction of the unmanned surface ship, and the abnormal positioning data that does not affect the linear fitting refers to the GNSS positioning data that does not obviously deviate from the true value of the geographic coordinates in the direction perpendicular to the motion direction of the unmanned surface ship, but seriously deviates from the true value of the geographic coordinates in the direction parallel to the linear motion direction of the unmanned surface ship. According to the characteristics that the abnormal positioning data that affect the linear fitting seriously deviates from the true value of the geographic coordinates in the direction perpendicular to the linear motion direction of the unmanned surface ship, the distance from each position data member in the positioning data set collected by the GNSS antenna to the linear motion path equation of the unmanned surface ship is solved, and the 3σ criterion in mathematical statistics is used to judge the degree of deviation of the distance error, so as to achieve accurate search and deletion of the abnormal positioning data that affect the linear fitting in the GNSS positioning data.
[0018] As a further improvement of the method of the present invention: the process of step S3 includes:
[0019] Step S31: The unmanned surface vessel moves along a linear path along the bow-stern line, and performs linear regression processing on the horizontal coordinate data and vertical coordinate data collected from antenna A and antenna B in the geodetic coordinate system; wherein the GNSS positioning data processing flow of antenna A is as follows:
[0020] Assume that the linear equation of the abscissa and ordinate data of antenna A is:
[0021] y=a 0 x+b 0 (1)
[0022] Among them, y represents the vertical coordinate data, x represents the horizontal coordinate data, and parameter a 0 ,b 0 is the coefficient to be solved in formula (1), and the positioning data set D A ={A 1 ,A 2 ,…,A i ,…,A n Substitute the horizontal coordinate data and vertical coordinate data in} into formula (1), according to the least squares criterion, a 0 ,b 0 The value of can be obtained by solving formula (2):
[0023]
[0024] in, is the ordinate data of antenna A, is the horizontal coordinate data of antenna A;
[0025] Step S32: Solve the positioning data set D according to the distance formula (3) from the point to the line A Position data member A i The distance to (1) is obtained as the distance data set is the i-th distance data of data set d:
[0026]
[0027] in, is the horizontal coordinate data of antenna A, is the vertical coordinate data of antenna A.
[0028] Step S33: Distance dataset According to formula (4), the mean value is calculated According to formula (5), the standard deviation s is calculated A :
[0029]
[0030]
[0031] Step S34: deleting abnormal GNSS positioning data values according to the 3σ criterion in mathematical statistics;
[0032] Step S35: Repeat steps S31-S34 to continue checking whether there are any abnormal positioning data values in the remaining GNSS positioning data until all abnormal data are deleted to obtain a GNSS positioning data set D that deletes the abnormal positioning data that affects the linear fitting. A ′={A 1 ′,A2 ′,…,A j ′,…,A m '}, where 1≤j≤m, m is a positive integer and m≤n, A' j To locate the dataset D A ′’s jth positioning data member, t j is the sampling time, t j The horizontal coordinate of antenna A in the geodetic coordinate system at this moment, t j The vertical coordinate data of antenna A at the moment. The same method is used to delete the abnormal positioning data that affects the linear fitting positioning data in the data collected by GNSS antenna B.
[0033] As a further improvement of the method of the present invention: in step S34, it is determined that the positioning data set D A The position data member A in i The distance to the linear fitting equation (1) Whether it satisfies formula (6), if it does, then A i Abnormal positioning data will be deleted; if it does not meet the requirements, it is considered to be valid data and will be retained:
[0034]
[0035] As a further improvement of the method of the present invention: in the step S4, according to the characteristic that the abnormal positioning data of the GNSS positioning data that does not affect the linear fitting seriously deviates from the true value of the geographic coordinates in the direction parallel to the linear motion direction of the unmanned surface ship, the rate value between two adjacent sampling time data members is solved in the GNSS positioning data set from which the abnormal positioning data that affects the linear fitting has been deleted, and the abnormal rate value is further judged by the 3σ criterion in mathematical statistics, so as to realize the accurate search and deletion of the positioning abnormal data that does not affect the linear fitting.
[0036] As a further improvement of the method of the present invention: the process of step S4 includes:
[0037] Step S41: Positioning data set D A 'Two adjacent positioning data members A' k ,A′ k+1 According to formula (7), the rate v is solved to obtain the rate data set Among them, 1≤k≤m v , m v is a positive integer and m v =m-1,v k is the kth data member in the data set v:
[0038]
[0039] Step S42: Calculate the mean of the rate data set according to equation (8): According to formula (9), the variance s of the rate data set is solved v :
[0040]
[0041]
[0042] Step S43: Using the 3σ criterion according to formula (10) to locate the data set D A ′ to locate abnormal data for judgment and deletion:
[0043]
[0044] When the rate v k If formula (9) is not satisfied, the positioning data set D A Two adjacent data members A′ in the sampling time k ,A′ k+1 If it is a valid value, it is retained; otherwise, it is considered that A′ k ,A′ k+1 is the abnormal location data to be detected. First, A′ k ,A′ k+1 From the positioning dataset D A ', and then locate the data A' k ,A′ k+1 The new data set to be tested is stored in the order of sampling time;
[0045] Step S44: Repeat steps S41-S43 until all abnormal GNSS positioning data to be detected are deleted, and the GNSS positioning data set JD after the data is deleted is obtained. A ′={JA 1 ′,JA 2 ′,…,JA j′ ′,…,JA m′ ′} and the GNSS dataset to be detected TD A ′={TA 1 ′,TA 2 ′,…,TA j″ ′,…,TA m″ '}, where 1≤j'≤m', 1≤j″≤m″, m' and m″ are both positive integers and m=m'+m″, JA' j′ To locate the dataset JD′ A The j′th position data member in t j′The horizontal coordinate of antenna A in the geodetic coordinate system at time, t j′ The vertical coordinate data of antenna A at time TA′ j″ To locate the dataset TD′ A The j″th position data member in t j″ The horizontal coordinate of antenna A in the geodetic coordinate system at time, t j″ The vertical coordinate data of antenna A at the moment;
[0046] Step S45: Set parameter I=1, and set the GNSS positioning data TA′ to be detected with serial number j″=I j″ Insert GNSS positioning dataset JD in sampling time order A ′;
[0047] Step S46: insert the GNSS dataset JD into which the data to be detected is inserted A ′, solve the rate between the data members adjacent to each other at the sampling time according to formula (6) to obtain the rate data set v′;
[0048] Step S47: Calculate the mean value of the velocity data set v′ according to equations (8) and (9) above: and variance s v′ ;
[0049] Step S48: According to formula (10), TA′ j″ The rate value between the adjacent positioning data points at the sampling time is judged. If it satisfies formula (10), it is considered that TA′ j″ To locate the anomaly data, from the JD dataset A ' is deleted, otherwise it is considered that TA' j″ It is a valid value and is retained;
[0050] Step S49: Let j″=I+1, repeat steps S45-S48, and determine the TD in the data set to be detected in turn. A ′={TA 1 ′,TA 2 ′,…,TA j″ ′,…,TA m″ ′}Whether each data member is abnormal positioning data, and finally obtain the valid GNSS positioning data set JD collected by GNSS antenna A A ′={JA 1 ′,JA 2 ′,…,JA j″′ ′,…,JA m″′ '}, where 1≤j″′≤m″′, m″′ is a positive integer and m′≤m″′≤m, JA′ j″′ To locate the dataset JD′A The j″′th position data member in t j″′ is the sampling time, t j″′ The horizontal coordinate of antenna A in the geodetic coordinate system at time, t j″′ The vertical coordinate data of antenna A at the moment; the same method is used to collect the positioning data D of antenna B B The deletion of abnormal positioning data does not affect the linear fitting.
[0051] As a further improvement of the method of the present invention: in the step S5, the GNSS system positioning data of antennas A and B at the same time with abnormal data deleted is used to solve the heading, which is compared with the heading data of the inertial navigation system at the same time to complete the calibration of the inertial navigation system heading.
[0052] Compared with the prior art, the advantages of the present invention are:
[0053] 1. The unmanned surface vessel dynamic heading measurement and calibration method of the present invention is a GNSS dual-antenna unmanned surface vessel dynamic heading measurement and calibration method based on linear path and abnormal positioning data deletion. It only needs to place the dual antennas on the bow and stern line to meet the measurement conditions. The equipment is fast and convenient to use. It adopts a GNSS-based radio measurement method, and can accurately measure anywhere covered by GNSS signals, without being affected by the measurement site and environmental visibility. In addition, GNSS measurement equipment is cheaper than precision optical equipment.
[0054] 2. The unmanned surface ship dynamic heading measurement calibration method of the present invention divides the abnormal positioning data in the GNSS system positioning data into abnormal positioning data that affects linear fitting and abnormal positioning data that does not affect linear fitting. According to the different degrees of deviation between the abnormal positioning data points that affect linear fitting and the effective positioning points and the linear path, a linear fitting model is constructed, and the 3σ criterion is used to accurately search and delete such abnormal positioning data; according to the actual situation that the time unified system of the unmanned surface ship can accurately synchronize the time before the work of the unmanned surface ship and the unmanned surface ship executes a uniform linear path, the rate value between the positioning data points of two adjacent sampling times is solved, and according to the different degrees of deviation between the abnormal positioning data points that do not affect linear fitting, the effective positioning points and the true value points, the 3σ criterion is used to accurately search and delete the abnormal positioning data points. Compared with the existing method based on Kalman filtering, the method of the present invention does not need to construct a complex mathematical model, does not need to accurately obtain the variance matrix of the relevant measurement noise, and realizes the search and deletion of positioning abnormal data based on the real-time measured GNSS data. The method has a small amount of calculation, low computational complexity, accurate search and deletion of abnormal data, and is simple and easy to implement.
[0055] 3. The unmanned surface vessel dynamic heading measurement and calibration method of the present invention can not only realize the dynamic heading measurement and calibration problem of the unmanned surface vessel, but also solve the heading measurement and calibration problems of other surface, air, land and other moving carriers under the premise of meeting the measurement conditions of the present method; because the method of the present invention can effectively delete abnormal positioning data on a linear path, it can also be applied to the measurement azimuth and measurement point accuracy calibration of radar, sonar and other measuring equipment; the method of the present invention has a wide range of applications and is simple to operate and easy to promote and implement.
[0056] 4. The purpose of the present invention is to reduce the impact of abnormal GNSS positioning data on the heading calibration of the inertial navigation system and improve the calibration accuracy of the heading. The technical solution of this invention is to construct a dual-antenna heading measurement system for the GNSS system. According to the characteristics of unmanned surface ships executing linear paths, the abnormal GNSS positioning data is cleverly divided into abnormal positioning data that affects linear fitting and abnormal positioning data that does not affect linear fitting. The 3σ criterion is used to judge the degree of deviation of the distance error from the positioning data to the linear path and the degree of deviation of the rate value error between the positioning data, so as to achieve the deletion of two types of abnormal positioning data and further achieve the heading calibration of the inertial navigation system.
[0057] In summary, the present invention develops a linear regression fitting model based on the characteristics of unmanned surface vessels performing linear motion, uses the 3σ criterion to achieve deletion of abnormal positioning data in the GNSS system, uses the effective positioning data of the GNSS system collected by antenna A and antenna B at the same time, calculates the heading data, compares it with the heading data of the inertial navigation system at the same time, and completes the calibration of the heading of the inertial navigation system. The method of the present invention has the advantages of wide application range, easy implementation, low cost, accurate search and deletion of abnormal positioning data, and is not restricted by external factors such as the geographical location of the measurement site and environmental visibility. BRIEF DESCRIPTION OF THE DRAWINGS
[0058] Figure 1 It is a schematic flow diagram of the method of the present invention.
[0059] Figure 2 It is a schematic diagram of the installation of the GNSS dual antenna in a specific application example of the present invention.
[0060] Figure 3 It is a schematic diagram of GNSS positioning anomaly points that affect linear fitting in a specific application example of the present invention.
[0061] Figure 4 It is a schematic diagram of GNSS positioning abnormal points that do not affect linear fitting in a specific application example of the present invention. DETAILED DESCRIPTION
[0062] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0063] The unmanned surface vessel dynamic heading measurement calibration method of the present invention is a heading measurement calibration method based on deletion of abnormal GNSS positioning data, and the steps include:
[0064] Step S1: Install GNSS dual antennas and use the timing function of the GNSS system to synchronize the time system of the unmanned surface ship;
[0065] like Figure 1 As shown, in this example, two GNSS antennas are respectively installed on the bow and stern lines of the unmanned surface ship, the GNSS antenna installed in the bow direction is marked as antenna A, and the GNSS antenna installed in the stern direction is marked as antenna B, so as to obtain a GNSS dual-antenna heading measurement system; the timing function of the GNSS system is used to synchronize the time unified system of the unmanned surface ship to ensure that all equipment on the unmanned surface ship works on the same time reference.
[0066] Step S2: When the unmanned ship performs linear path navigation, the heading data of the inertial navigation system and the positioning data of the GNSS dual antennas at the same time are collected;
[0067] In a specific application example, the unmanned surface ship performs linear motion along the bow-stern line direction at a constant speed V, and uses the same sampling time and sampling frequency to collect the GNSS positioning data at the positions of antenna A and antenna B and the heading data of the inertial navigation system to obtain the heading data set H = {h 1 ,h 2 ,…,h i ,…,h n}、GNSS antenna A positioning data set D A ={A 1 ,A 2 ,…,A i ,…,A n}, positioning data set D of GNSS system antenna B B = {B 1 ,B 2 ,…,B i ,…,B n}; where 1≤i≤n, n is a positive integer, h i is the i-th member data in the heading data set H, h i ={t i ,f i}, t i is the sampling time, f i t i Heading data collected at all times; A i To locate the dataset D AThe i-th position data member in t i The horizontal coordinate of antenna A in the geodetic coordinate system at this moment, t i The vertical coordinate of antenna A at the moment; B i To locate the dataset D B The i-th member data in t i The horizontal coordinate of antenna B in the geodetic coordinate system at time, t i The vertical coordinate of antenna B at time.
[0068] Step S3: Delete abnormal data that affects linear fitting;
[0069] Abnormal positioning data that affect linear fitting refers to GNSS positioning data that seriously deviates from the true value of geographic coordinates in the direction perpendicular to the linear motion direction of the unmanned surface ship, such as Figure 3 The black rectangle “■” position point shown; Figure 3 In the figure, the arrow indicates the direction of movement of the unmanned surface ship, the solid line GH is the linear movement path of the unmanned surface ship, and the black solid circle “●” position point is the effective positioning data point of the GNSS antenna within the allowable range of measurement error; Figure 3 It can be seen that the black rectangle “■” position point deviates seriously from the linear path GH compared with the valid positioning point. In order to avoid the introduction of errors in the calibration of the unmanned surface ship heading by abnormal positioning data that affect the linear fitting, the present invention constructs a motion path equation solution model based on linear regression. According to the characteristics that the black rectangle abnormal positioning point deviates seriously from the true value of the geographic coordinates in the direction perpendicular to the linear motion of the unmanned surface ship, the distance from each GNSS positioning data point to the linear motion trajectory equation of the unmanned surface ship is solved, and the distance value is judged by the 3σ criterion in mathematical statistics, so as to realize the accurate search and deletion of abnormal positioning data points that affect the linear fitting in the GNSS positioning data.
[0070] Step S4: deleting abnormal positioning data that does not affect linear fitting;
[0071] Abnormal positioning data that does not affect linear fitting refers to GNSS positioning data that does not significantly deviate from the true value of geographic coordinates in the direction perpendicular to the motion direction of the unmanned surface ship, but seriously deviates from the true value of geographic coordinates in the direction parallel to the linear motion direction of the unmanned surface ship, such as Figure 4 The black solid rectangle “■” shown is the position point A′; Figure 4In the figure, the hollow circle “○” position point A and the hollow circle “○” position point B are the true position points of antenna A and antenna B respectively, the black solid circle “●” is the effective positioning data point of antenna A within the allowable range of measurement error, the black solid rectangle “■” position point A′ is the positioning abnormal data point of antenna A, θ is the true value of the heading of the unmanned surface ship, θ 1 is the heading of the unmanned surface ship calculated by the positioning abnormal data point A′ of antenna A, θ 2 is the heading measured by the effective positioning data of antenna A, given by Figure 4 It can be seen that, with θ 2 Compared with θ 1 Severe deviation from the true value θ, in order to reduce the influence of abnormal positioning data that does not affect linear fitting on heading calibration, the present invention solves the rate value between the positioning data at two adjacent sampling moments, and uses the 3σ criterion in mathematical statistics to judge the error deviation degree of the rate value, thereby realizing accurate search and deletion of abnormal positioning data points that do not affect linear fitting in GNSS positioning data.
[0072] Step S5: Using the effective positioning data of the dual antennas of the GNSS system at the same time, the heading of the unmanned surface ship is solved and compared with the heading data of the inertial navigation system at the same time to achieve calibration of the heading of the inertial navigation system.
[0073] In a specific application example, the present invention uses the effective GNSS system positioning data of antenna A and antenna B at the same time to solve the heading at that time, and compares it with the heading data of the inertial navigation system at the same time to achieve calibration of the heading of the inertial navigation system.
[0074] In a specific application example, the detailed process of step S3 includes:
[0075] Step S31: The unmanned surface vessel performs linear path movement along the bow-stern line direction, and performs linear regression processing on the horizontal coordinate and vertical coordinate data of antenna A and antenna B in the geodetic coordinate system; wherein, the processing flow of the GNSS positioning data collected by antenna A is as follows:
[0076] Assume that the linear equations of the abscissa and ordinate data of antenna A in the geodetic coordinate system are:
[0077] y=a 0 x+b 0 (1)
[0078] Among them, y represents the ordinate data of antenna A, x represents the abscissa data of antenna A, and parameter a 0 ,b 0 is the coefficient to be solved in equation (1), and the collected positioning data set D ASubstitute the horizontal and vertical coordinate data in (1) and according to the least squares criterion, a 0 ,b 0 The value of can be obtained by solving formula (2):
[0079]
[0080] in, is the vertical coordinate data of antenna A in the geodetic coordinate system, is the horizontal coordinate of antenna A in the geodetic coordinate system;
[0081] Step S32: Solve the positioning data set D of GNSS antenna A according to the distance formula (3) from point to line A Position data member A i The distance to (1) is obtained as the distance data set is the i-th distance data of data set d:
[0082]
[0083] in, To locate the dataset D A Position data member A i The horizontal coordinate in the geodetic coordinate system is: To locate the dataset D A Position data member A i The vertical coordinate data.
[0084] Step S33: Distance dataset According to formula (4), the mean value is calculated According to formula (5), the standard deviation s is calculated A :
[0085]
[0086]
[0087] Step S34: Delete abnormal GNSS positioning data values according to the 3σ criterion in mathematical statistics; the specific process is as follows:
[0088] Determine the positioning data set D A The position data member A in i The distance to (1) Whether it satisfies formula (6), if it does, then A i To locate the anomaly data, from the dataset D A If it is not satisfied, it is considered that A i This is valid data and will be retained:
[0089]
[0090] Step S35: Repeat steps S31-S34 to continue checking the remaining GNSS positioning data D A Whether there are still abnormal positioning data in the data, until all the abnormal positioning data are deleted, and the GNSS positioning data set D that deletes the abnormal positioning data that affect the linear fitting is obtained. A ′={A 1 ′,A 2 ′,…,A j ′,…,A m '}, where 1≤j≤m, m is a positive integer and m≤n, A' j To locate the dataset D A ′’s jth positioning data member, t j is the sampling time, t j The horizontal coordinate of antenna A in the geodetic coordinate system at this moment, t j The vertical coordinate data of antenna A at the moment. The same method is used to eliminate the abnormal positioning data that affects the linear fitting positioning data in the data collected by GNSS antenna B.
[0091] In a specific application example, the detailed process of step S4 includes:
[0092] Step S41: GNSS positioning data set D A The location data member A′ of two adjacent sampling moments in ′ k ,A′ k+1 According to the rate value calculated by formula (7), the rate data set is obtained Among them, 1≤k≤m v , m v is a positive integer and m v =m-1,v k is the kth data member in the data set v:
[0093]
[0094] Step S42: Calculate the mean of the velocity data set v according to equation (8): According to formula (9), the standard deviation s of the rate data set v is solved v :
[0095]
[0096]
[0097] Step S43: Using the 3σ criterion according to formula (10) to locate the data set D A′ to locate abnormal data for judgment and deletion:
[0098]
[0099] When the rate v k If the formula (9) is not satisfied, it is considered that A′ k ,A′ k+1 If it is a valid value, it is retained; otherwise, it is considered that A′ k ,A′ k+1 is the abnormal location data to be detected. First, A′ k ,A′ k+1 From the positioning dataset D A ', and then locate the data A' k ,A′ k+1 The new data set to be tested is stored in the order of sampling time;
[0100] Step S44: Repeat steps S41-S43 until all abnormal GNSS positioning data to be detected are eliminated to obtain a GNSS positioning data set JD after the data is eliminated. A ′={JA 1 ′,JA 2 ′,…,JA j′ ′,…,JA m′ ′} and the GNSS dataset to be detected TD A ′={TA 1 ′,TA 2 ′,…,TA j″ ′,…,TA m″ '}, where 1≤j'≤m', 1≤j″≤m″, m' and m″ are both positive integers and m=m'+m″, JA' j′ To locate the dataset JD′ A The j′th position data member in t j′ The horizontal coordinate of antenna A in the geodetic coordinate system at this moment, t j′ The vertical coordinate data of antenna A in the geodetic coordinate system at this moment; To locate the dataset TD′ A The j″th position data member in t j″ The horizontal coordinate of antenna A in the geodetic coordinate system at this moment, t j″ The ordinate of antenna A in the geodetic coordinate system at this moment;
[0101] Step S45: Set parameter I=1, and set the GNSS positioning data TA′ to be detected with serial number j″=I j″Insert GNSS positioning dataset JD in sampling time order A ′;
[0102] Step S46: insert the GNSS dataset JD into which the data to be detected is inserted A ′, solve the rate between the data members adjacent to each other at the sampling time according to formula (6) to obtain the rate data set v′;
[0103] Step S47: Calculate the mean of the velocity data set v′ according to equations (8) and (9): and variance s v′ ;
[0104] Step S48: According to formula (10), TA′ j″ The rate value between the adjacent positioning data points at the sampling time is judged. If it satisfies formula (10), it is considered that TA′ j″ To locate the anomaly data, from the JD dataset A ' is deleted, otherwise it is considered that TA' j″ It is a valid value and is retained;
[0105] Step S49: Let j″=I+1, repeat steps S45-S48, and determine the TD in the data set to be detected in turn. A ′={TA 1 ′,TA 2 ′,…,TA j″ ′,…,TA m″ ′}Whether each data member is abnormal positioning data, and finally obtain the valid GNSS positioning data set JD collected by GNSS antenna A A ′={JA 1 ′,JA 2 ′,…,JA j″′ ′,…,JA m″′ '}, where 1≤j″′≤m″′, m″′ is a positive integer and m′≤m″′≤m, JA′ j″′ To locate the dataset JD′ A The j″′th position data member in t j″′ is the sampling time, t j″′ The horizontal coordinate data of antenna A in the geodetic coordinate system at this moment, t j″′ The vertical coordinate data of antenna A in the geodetic coordinate system at the moment; the same method is used to delete the abnormal positioning data in the positioning data collected by antenna B without affecting the linear fitting.
[0106] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.
Claims
1. A method for calibrating the dynamic heading measurement of an unmanned surface vessel, characterized in that the steps include: Step S1: Install GNSS dual antennas and use the timing function of the GNSS system to synchronize the time of the unmanned surface ship's unified system; Step S2: While the unmanned ship is sailing along the linear motion path, the heading data of the inertial navigation system and the positioning data at the positions of the GNSS dual antennas are collected at the same time; Step S3: Deleting abnormal GNSS positioning data that affects linear fitting; Step S4: deleting abnormal GNSS positioning data that does not affect linear fitting; Step S5: using the effective positioning data collected by the GNSS dual antenna system at the same time, the heading data is calculated and compared with the heading data of the inertial navigation system at the same time to achieve the calibration of the inertial navigation system; In step S4, according to the characteristic that the abnormal GNSS positioning data that does not affect the linear fitting seriously deviates from the true value of the geographic coordinates in the direction parallel to the linear motion direction of the unmanned surface ship, the GNSS positioning data set from which the abnormal positioning data that affects the linear fitting has been deleted is solved. The rate value between two adjacent sampling time points in the The criterion judges the rate value of the abnormal positioning data point to achieve accurate search and deletion of abnormal positioning data that does not affect the linear fitting.
2. The unmanned surface vessel dynamic heading measurement calibration method according to claim 1, characterized in that: In the step S1, two GNSS antennas are respectively installed on the bow and stern lines of the unmanned surface ship, the GNSS antenna installed in the bow direction is marked as antenna A, and the GNSS antenna installed in the stern direction is marked as antenna B, so as to obtain a GNSS system dual-antenna heading measurement system; the timing function of the GNSS system is used to synchronize the time unified system of the unmanned surface ship to ensure that all equipment on the unmanned surface ship operates on the same time reference.
3. The unmanned surface vessel dynamic heading measurement and calibration method according to claim 2, characterized in that: In step S2, the unmanned surface vessel moves at a constant speed. Perform linear motion along the bow-stern line direction, use the same sampling time and sampling frequency to collect the GNSS positioning data at the positions of antenna A and antenna B and the heading data of the inertial navigation system, and obtain the heading data set of the inertial navigation system , positioning data set of antenna A , positioning data set of antenna B ;in, , is a positive integer, Heading dataset The Member data, , is the sampling time, for Heading data collected at all times; To locate the dataset The Position data members, , for The horizontal coordinate of antenna A in the geodetic coordinate system at this moment, for The ordinate of antenna A in the geodetic coordinate system at this moment; To locate the dataset The Member data, , for The horizontal coordinate of antenna B in the geodetic coordinate system at this moment, for The vertical coordinate of antenna B at time.
4. The unmanned surface vessel dynamic heading measurement and calibration method according to any one of claims 1 to 3, characterized in that: In step S3, the positioning data collected by the GNSS antenna is processed by a linear regression method to obtain a linear motion path equation of the unmanned surface vessel.
5. The unmanned surface vessel dynamic heading measurement and calibration method according to claim 4, characterized in that: The process of step S3 includes: Step S31: The unmanned surface vessel performs linear path movement along the bow-stern line direction, and performs linear regression processing on the horizontal coordinate data and vertical coordinate data collected from antenna A and antenna B in the geodetic coordinate system; wherein the GNSS positioning data processing flow of antenna A is as follows: Assume that the linear equation of the abscissa data and ordinate data of antenna A is: in, represents the ordinate of antenna A, Represents the horizontal coordinate of antenna A, parameter is the coefficient to be solved in equation (1), and the collected positioning data set Substitute the horizontal and vertical coordinate data into formula (1) and according to the least squares criterion, The value of can be obtained by solving formula (2): in, , is the ordinate data of antenna A, is the horizontal coordinate data of antenna A; Step S32: Calculate the positioning data set according to the distance formula (3) from the point to the line Position data member The distance to (1) is obtained as the distance data set , For the dataset No. Distance data: in, is the horizontal coordinate data of antenna A, is the ordinate data of antenna A; Step S33: Distance dataset According to (4), find the mean , calculate the standard deviation according to formula (5) : Step S34: According to mathematical statistics The criterion deletes the abnormal positioning data that affects the linear fitting; Step S35: Repeat steps S31-S34 to continue checking the remaining GNSS positioning data sets Whether there are still abnormal positioning data in the data, until all abnormal positioning data are deleted, and the GNSS positioning data set that deletes the abnormal positioning data that affects the linear fitting is obtained. ,in, , is a positive integer and , To locate the dataset No. Positioning data members, , is the sampling time, for The horizontal coordinate data of antenna A in the geodetic coordinate system at this moment, for The ordinate data of antenna A at the moment; the same method is used to delete the abnormal positioning data that affects the linear fitting positioning data in the data collected by GNSS antenna B.
6. The unmanned surface vessel dynamic heading measurement and calibration method according to claim 5, characterized in that: In step S34, the positioning data set is determined The position data member in The distance to the linear fitting equation (1) Whether it satisfies formula (6), if it does, then it is considered Abnormal positioning data will be deleted; if it does not meet the requirements, it is considered to be valid data and will be retained: 。 7. The unmanned surface vessel dynamic heading measurement and calibration method according to claim 1, characterized in that: The process of step S4 includes: Step S41: Data set The location data members of two adjacent sampling times in The rate calculated by formula (7) , get the rate data set ,in, , is a positive integer and , For the dataset The Data members: Step S42: Solve the rate data set according to equation (8) The mean , solve the standard deviation of the rate data set according to formula (9): : Step S43: Use The criterion is to locate the data set according to formula (10): Locate abnormal data for judgment and deletion: When the rate If formula (9) is not satisfied, it is considered If it is a valid value, it is retained; otherwise, it is considered is the abnormal location data to be detected. From the positioning dataset , and then Store the new data set to be tested in the order of sampling time ; Step S44: Repeat steps S41-S43 until the positioning data set is deleted All abnormal GNSS positioning data to be detected in the data set are deleted to obtain the GNSS positioning data set after the data is deleted. And the GNSS dataset to be tested ,in, , and are all positive integers and , To locate the dataset The Position data members, , is the sampling time, for The horizontal coordinate of antenna A in the geodetic coordinate system at this moment, for The vertical coordinate data of antenna A at the moment; To locate the dataset The Position data members, , is the sampling time, for The horizontal coordinate data of antenna A in the geodetic coordinate system at this moment, for The vertical coordinate data of antenna A at the moment; Step S45: Set parameter I=1, and set the serial number GNSS positioning data to be detected Incorporate into the GNSS positioning data set in the order of sampling time ; Step S46: insert the GNSS data set into which the data to be detected is inserted , solve the rate between adjacent positioning data members at the sampling time according to formula (6) to obtain the rate data set ; Step S47: Solve the rate data set according to equations (8) and (9) above Mean and variance ; Step S48: According to formula (10), The rate value between the adjacent positioning data points at the sampling time is judged. If it satisfies formula (10), it is considered To locate the anomaly data, from the dataset be deleted, otherwise it is considered It is a valid value and is retained; Step S49: , repeat steps S45-S48, and judge the Whether each data member is abnormal positioning data, and finally obtain the valid GNSS positioning data set of GNSS antenna A ,in, , is a positive integer and , To locate the dataset The Position data members, , is the sampling time, for The horizontal coordinate of antenna A in the geodetic coordinate system at time, for The ordinate data of antenna A at the moment; the same method is used to delete the abnormal positioning data that does not affect the linear fitting in the positioning data collected by antenna B.
8. The unmanned surface vessel dynamic heading measurement and calibration method according to any one of claims 1 to 3, characterized in that: In step S5, the heading data obtained by solving the GNSS system positioning data collected by antennas A and B at the same time and from which abnormal data has been deleted is compared with the heading data of the inertial navigation system at the same time to complete the calibration of the heading of the inertial navigation system.
Citation Information
Patent Citations
East gyro drift and heading error calibration method for inertial navigation system
CN112525216A
Heading measurement compensation for GNSS navigation
US10512204B1
