Multi-parameter online calibration method for Doppler velocimeter
阅读说明：本技术 一种多普勒测速仪多参数在线标定方法 (Multi-parameter online calibration method for Doppler velocimeter ) 是由 侯斌 年海涛 王彬 杨超平 于 2019-11-21 设计创作，主要内容包括：本发明涉及一种多普勒测速仪多参数在线标定方法,其特征在于：所述标定方法的步骤为：1)系统准备；2)算法计算。本发明设计可降低传统多普勒测速标定方案对航行轨迹和参考信息精度、标定时间等苛刻要求,同时可以多标定出两个安装角误差,并可以实现在线标定；提高了多普勒标定技术的精度和应用范围,同时可以提高水下导航定位精度。(The invention relates to a multi-parameter online calibration method of a Doppler velocimeter, which is characterized by comprising the following steps: the calibration method comprises the following steps: 1) preparing a system; 2) and (4) calculating an algorithm. The design of the invention can reduce the rigorous requirements of the traditional Doppler speed measurement calibration scheme on navigation track and reference information precision, calibration time and the like, can calibrate more errors of two installation angles, and can realize online calibration; the accuracy and the application range of the Doppler calibration technology are improved, and meanwhile, the underwater navigation positioning accuracy can be improved.)
1. A multi-parameter online calibration method for a Doppler velocimeter is characterized by comprising the following steps: the calibration method comprises the following steps:
1) preparing a system: after the inertial navigation equipment is initially aligned, connecting with GNSS external reference data and Doppler speed measurement equipment, sailing the ship according to a pre-designed calibration scheme track, simultaneously storing the velocity information of the Doppler northeast weather decomposed by the inertial navigation equipment by using a data storage device, simultaneously recording the northeast weather information output by the GNSS, and ensuring that the recorded data of the two equipment reflect the velocity at the same moment through GNSS space/time;
2) and (3) algorithm calculation: then, after the ship finishes sailing of the calibration path, calculating the calibration parameters of Doppler by using an algorithm; meanwhile, an online calibration mode can be adopted, the calibration algorithm is embedded into a resolving computer of the inertial navigation system, real-time calibration is carried out during ship navigation, and Doppler parameters can be calibrated again in real time by using floating gaps of underwater equipment when the Doppler use environment is obviously changed.
2. The Doppler velocimeter multi-parameter online calibration method according to claim 1, characterized in that: the algorithm in the step 2) comprises the following steps:
a. doppler error modeling
Setting the installation angle between Doppler and inertial navigation as a small angle, and according to the small angle approximation principle, setting the installation angle matrix as
α thereinx,αy,αzRespectively forming installation included angles in the pitching direction, the rolling direction and the azimuth direction of the Doppler and inertial navigation coordinate systems;
the velocity of the Doppler measurement can be converted into the velocity of the northeast sky under a navigation coordinate system through inertial navigation attitude matrix decomposition:
wherein: v. ofdopR、vdopLThe lateral and forward velocities of the doppler velocimetry output are respectively;
r, P, H is roll angle, pitch angle, and course angle output by inertial navigation system;
b. inertial navigation/Doppler/GNSS calibration filtering algorithm
(1) Kalman filtering one-step prediction
Divided into state transition matrix phik,k-1Is input to a noise variance matrix
i. state transition matrix phik,k-1Is calculated by
Note (t)k-1,tk]For a prediction period, h ═ tk－tk-1The prediction period h is generally short, and the state transition matrix is calculated as follows
ii. Input noise variance matrix
The covariance matrix of the system noise of the continuous system, i.e. three gyros and three accelerometer vectors W (t), is Q (t), and the covariance matrix of the input noise is Q (t)
Qq＝G(t)Q(t)GT(t)
Where q (t) is a constant, g (t) is a noise input matrix, and the following are rewritten:
Q＝diag[(0.01°/h)2(0.01°/h)2(0.01°/h)2(0.1m/s)2]
obtaining the noise variance Q of continuous system elementsqPost-computation Kalman discretization form
state prediction at time iii and K
When the initial time when k is 0,
P0＝diag[(100°)2(100°)2(100°)2(1m/s)2]
when k is 0, 1, 2, …, recursion calculation
When the filtering updating period is not reached, the prediction updating is carried out
Pk＝Pk,k-1
When the filter update period is over, the filter update period,
(2) kalman filter update
The filtering update period is equal to the outer reference information update period, where the GNSS effective information update frequency is 1Hz, so the filtering update period is set to 1s, and the calculation is divided into four steps:
i. metrology calculations
The measured values were calculated as follows:
the subscript s represents the northeast speed output by resolving the Doppler sum speed through the attitude matrix of the strapdown inertial navigation system, i.e.
Wherein: v. ofdopR、vdopLLateral and forward velocities output for doppler velocimetry;
the subscript r denotes the northeast speed of the reference GNSS output;
vsE、vsN、vsUeast speed, north speed and vertical speed output by Doppler velocity measurement, unit: m/s;
vrE、vrN、vrUreference speed, unit for GNSS output: m/s;
ii. Filter gain calculation
The filter gain K is calculated as followsk：
Wherein: pk,k-1Calculating error variance prediction;
Rk＝diag[(0.3m/s)2(0.3m/s)2(0.3m/s)2]。
iii, state estimation update
State estimation is calculated as follows
Wherein the content of the first and second substances,calculating for the state prediction;
iv error variance update
The error variance P is calculated as followsk：
Technical Field
The invention belongs to the technical field of underwater inertial navigation positioning and orientation, relates to online calibration of multiple Doppler parameters by using inertial navigation and satellite navigation, and particularly relates to a multi-parameter online calibration method of a Doppler velocimeter.
Background
In the underwater unmanned aerial vehicle service environment such as UAV, based on the reasons of volume, power consumption and cost and service environment's restriction, the well, low accuracy inertial navigation equipment and the GNSS assistance-localization real-time that its navigation system generally adopted to after a series of preparation work such as unmanned aerial vehicle accomplishes initial alignment under water, generally can be in long-term underwater state of diving, GNSS system is unusable this moment, and its location error of well, low accuracy inertial navigation can be exponential growth along with time, in order to guarantee underwater positioning accuracy, need with the help of Doppler speedometer or long and short baseline assistance-localization real-time. In the actual use process of Doppler, the output speed measurement information and the output of the inertial navigation system are not on the same coordinate system, and the Doppler speed measurement precision is influenced by ocean current, temperature, installation mode and the like. Therefore, before implementing combined positioning of the doppler/inertial navigation system, the installation angle between the doppler and the inertial navigation system and the scaling factor of the doppler itself need to be calibrated in advance.
The invention utilizes an inertial system and GNSS navigation, combines Doppler velocity information, and can accurately calibrate three installation angles and scale factors of Doppler by a combined navigation algorithm, compared with the traditional Doppler two-point calibration or constant-speed direct navigation calibration scheme, the invention reduces the rigorous requirements of the calibration process on ship navigation distance, navigation track and reference position, and particularly in the application field of underwater unmanned aerial vehicles, the invention can utilize the gap of the unmanned aerial vehicle floating upwards in short time, realizes the online calibration and correction of Doppler parameters, and can scale the errors of the other two installation angles of Doppler, and can improve the positioning precision of combined navigation.
Disclosure of Invention
The invention aims to overcome the defects of the prior art, provides a multi-parameter online calibration method of a Doppler velocimeter, and solves the problems that the prior Doppler calibration scheme has strict requirements on external reference and navigation tracks, and the calibrated parameters are limited and have low precision.
The technical problem to be solved by the invention is realized by the following technical scheme:
a multi-parameter online calibration method for a Doppler velocimeter is characterized by comprising the following steps: the calibration method comprises the following steps:
1) preparing a system: after the inertial navigation equipment is initially aligned, connecting with GNSS external reference data and Doppler speed measurement equipment, sailing the ship according to a pre-designed calibration scheme track, simultaneously storing the velocity information of the Doppler northeast weather decomposed by the inertial navigation equipment by using a data storage device, simultaneously recording the northeast weather information output by the GNSS, and ensuring that the recorded data of the two equipment reflect the velocity at the same moment through GNSS space/time;
2) and (3) algorithm calculation: then, after the ship finishes sailing of the calibration path, calculating the calibration parameters of Doppler by using an algorithm; meanwhile, an online calibration mode can be adopted, the calibration algorithm is embedded into a resolving computer of the inertial navigation system, real-time calibration is carried out during ship navigation, and Doppler parameters can be calibrated again in real time by using floating gaps of underwater equipment when the Doppler use environment is obviously changed.
Moreover, the algorithm in the step 2) comprises:
a. doppler error modeling
Setting the installation angle between Doppler and inertial navigation as a small angle, and according to the small angle approximation principle, setting the installation angle matrix as
α thereinx,αy,αzRespectively forming installation included angles in the pitching direction, the rolling direction and the azimuth direction of the Doppler and inertial navigation coordinate systems;
the velocity of the Doppler measurement can be converted into the velocity of the northeast sky under a navigation coordinate system through inertial navigation attitude matrix decomposition:
wherein: v. ofdopR、vdopLThe lateral and forward velocities of the doppler velocimetry output are respectively;
r, P, H is roll angle, pitch angle, and course angle output by inertial navigation system;
b. inertial navigation/Doppler/GNSS calibration filtering algorithm
(1) Kalman filtering one-step prediction
Divided into state transition matrix phik,k-1Is input to a noise variance matrix
i. state transition matrix phik,k-1Is calculated by
Note (t)k-1,tk]For a prediction period, h ═ tk－tk-1The prediction period h is generally short, and the state transition matrix is calculated as follows
ii. Input noise variance matrix
The covariance matrix of the system noise of the continuous system, i.e. three gyros and three accelerometer vectors W (t), is Q (t), and the covariance matrix of the input noise is Q (t)
Qq＝G(t)Q(t)GT(t)
Where q (t) is a constant, g (t) is a noise input matrix, and the following are rewritten:
Q＝diag[(0.01°/h)2(0.01°/h)2(0.01°/h)2(0.1m/s)2]
obtaining the noise variance Q of continuous system elementsqPost-computation Kalman discretization form
state prediction at time iii and K
When the initial time when k is 0,
P0＝diag[(100°)2(100°)2(100°)2(1m/s)2]
when k is 0, 1, 2, …, recursion calculation
When the filtering updating period is not reached, the prediction updating is carried out
Pk＝Pk,k-1
When the filter update period is over, the filter update period,
(2) kalman filter update
The filtering update period is equal to the outer reference information update period, where the GNSS effective information update frequency is 1Hz, so the filtering update period is set to 1s, and the calculation is divided into four steps:
i. metrology calculations
The measured values were calculated as follows:
the subscript s represents the northeast speed output by resolving the Doppler sum speed through the attitude matrix of the strapdown inertial navigation system, i.e.
Wherein: v. ofdopR、vdopLLateral and forward velocities output for doppler velocimetry;
the subscript r denotes the northeast speed of the reference GNSS output;
vsE、vsN、vsUeast speed, north speed and vertical speed output by Doppler velocity measurement, unit: m/s;
vrE、vrN、vrUreference speed, unit for GNSS output: m/s;
ii. Filter gain calculation
The filter gain K is calculated as followsk：
Wherein: pk,k-1Calculating error variance prediction;
Rk＝diag[(0.3m/s)2(0.3m/s)2(0.3m/s)2]。
iii, state estimation update
State estimation is calculated as follows
Wherein the content of the first and second substances,
iv error variance update
The error variance P is calculated as followsk：
The invention has the advantages and beneficial effects that:
1. the multi-parameter online calibration method of the Doppler velocimeter can reduce the harsh requirements of the traditional Doppler velocimetry calibration scheme on navigation track and reference information precision, calibration time and the like, can calibrate two more installation angle errors simultaneously, and can realize online calibration. The accuracy and the application range of the Doppler calibration technology are improved, and meanwhile, the underwater navigation positioning accuracy can be improved.
Drawings
FIG. 1 is a basic schematic diagram of a multi-parameter online calibration method of a Doppler velocimeter of the present invention;
fig. 2 is a flow chart of the algorithm of the multi-parameter online calibration method of the doppler velocimeter of the present invention.
Detailed Description
The present invention is further illustrated by the following specific examples, which are intended to be illustrative, not limiting and are not intended to limit the scope of the invention.
A three-axis gyroscope and a three-axis accelerometer form a strapdown inertial navigation system, inertial navigation resolving is carried out, and attitude information is output to be used for decomposing Doppler speed measurement information; establishing a Doppler output error mathematical model, estimating installation errors and calibration scale factors between Doppler and inertial navigation by using GNSS speed measurement information and a Kalman filtering method, wherein in order to shorten calibration time, a ship navigation track can adopt a 'return' shape or a 'step' type motion, so that a Doppler calibration coefficient can be calibrated in a short time, and the basic principle is shown in figure 1; the specific algorithm is shown in fig. 2.
A multi-parameter online calibration method for a Doppler velocimeter is characterized by comprising the following steps: the calibration method comprises the following steps:
1) preparing a system: after the inertial navigation equipment is initially aligned, connecting with GNSS external reference data and Doppler speed measurement equipment, sailing the ship according to a pre-designed calibration scheme track, simultaneously storing the velocity information of the Doppler northeast weather decomposed by the inertial navigation equipment by using a data storage device, simultaneously recording the northeast weather information output by the GNSS, and ensuring that the recorded data of the two equipment reflect the velocity at the same moment through GNSS space/time;
2) and (3) algorithm calculation: then, after the ship finishes sailing of the calibration path, calculating the calibration parameters of Doppler by using an algorithm; meanwhile, an online calibration mode can be adopted, the calibration algorithm is embedded into a resolving computer of the inertial navigation system, real-time calibration is carried out during ship navigation, and Doppler parameters can be calibrated again in real time by using floating gaps of underwater equipment when the Doppler use environment is obviously changed.
Moreover, the algorithm in the step 2) comprises:
a. doppler error modeling
Setting the installation angle between Doppler and inertial navigation as a small angle, and according to the small angle approximation principle, setting the installation angle matrix as
α thereinx,αy,αzRespectively forming installation included angles in the pitching direction, the rolling direction and the azimuth direction of the Doppler and inertial navigation coordinate systems;
the velocity of the Doppler measurement can be converted into the velocity of the northeast sky under a navigation coordinate system through inertial navigation attitude matrix decomposition:
wherein: v. ofdopR、vdopLThe lateral and forward velocities of the doppler velocimetry output are respectively;
r, P, H is roll angle, pitch angle, and course angle output by inertial navigation system;
is the projection of Doppler in the northeast direction of the navigation coordinate system;
b. inertial navigation/Doppler/GNSS calibration filtering algorithm
(1) Kalman filtering one-step prediction
Divided into state transition matrix phik,k-1Meter (2)Computing, inputting noise variance matrix
i. state transition matrix phik,k-1Is calculated by
Note (t)k-1,tk]For a prediction period, h ═ tk－tk-1The prediction period h is generally short, and the state transition matrix is calculated as follows
ii. Input noise variance matrix
The covariance matrix of the system noise of the continuous system, i.e. three gyros and three accelerometer vectors W (t), is Q (t), and the covariance matrix of the input noise is Q (t)
Qq＝G(t)Q(t)GT(t)
Where q (t) is a constant, g (t) is a noise input matrix, and the following are rewritten:
Q＝diag[(0.01°/h)2(0.01°/h)2(0.01°/h)2(0.1m/s)2]
obtaining the noise variance Q of continuous system elementsqPost-computation Kalman discretization form
state prediction at time iii and K
When the initial time when k is 0,
P0＝diag[(100°)2(100°)2(100°)2(1m/s)2]
when k is 0, 1, 2, …, recursion calculation
When the filtering updating period is not reached, the prediction updating is carried out
Pk＝Pk,k-1
When the filter update period is over, the filter update period,
(2) kalman filter update
The filtering update period is equal to the outer reference information update period, where the GNSS effective information update frequency is 1Hz, so the filtering update period is set to 1s, and the calculation is divided into four steps:
i. metrology calculations
The measured values were calculated as follows:
the subscript s represents the northeast speed output by resolving the Doppler sum speed through the attitude matrix of the strapdown inertial navigation system, i.e.
Wherein: v. ofdopR、vdopLLateral and forward velocities output for doppler velocimetry;
the subscript r denotes the northeast speed of the reference GNSS output;
vsE、vsN、vsUeast speed, north speed and vertical speed output by Doppler velocity measurement, unit: m/s;
vrE、vrN、vrUreference speed, unit for GNSS output: m/s;
ii. Filter gain calculation
The filter gain K is calculated as followsk：
Wherein: pk,k-1Calculating error variance prediction;
Rk＝diag[(0.3m/s)2(0.3m/s)2(0.3m/s)2]。
iii, state estimation update
Calculated by the following formulaState estimation
Wherein the content of the first and second substances,calculating for the state prediction;
iv error variance update
The error variance P is calculated as followsk：
Although the embodiments of the present invention and the accompanying drawings are disclosed for illustrative purposes, those skilled in the art will appreciate that: various substitutions, changes and modifications are possible without departing from the spirit and scope of the invention and the appended claims, and therefore the scope of the invention is not limited to the disclosure of the embodiments and the accompanying drawings.