CN104280026B - Deepwater robot Long baselines Combinated navigation method based on AUKF - Google Patents

Deepwater robot Long baselines Combinated navigation method based on AUKF Download PDF

Info

Publication number
CN104280026B
CN104280026B CN201310284939.0A CN201310284939A CN104280026B CN 104280026 B CN104280026 B CN 104280026B CN 201310284939 A CN201310284939 A CN 201310284939A CN 104280026 B CN104280026 B CN 104280026B
Authority
CN
China
Prior art keywords
msub
mrow
msubsup
msup
filter
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201310284939.0A
Other languages
Chinese (zh)
Other versions
CN104280026A (en
Inventor
刘开周
李静
郭威
祝普强
王晓辉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenyang Institute of Automation of CAS
Original Assignee
Shenyang Institute of Automation of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenyang Institute of Automation of CAS filed Critical Shenyang Institute of Automation of CAS
Priority to CN201310284939.0A priority Critical patent/CN104280026B/en
Publication of CN104280026A publication Critical patent/CN104280026A/en
Application granted granted Critical
Publication of CN104280026B publication Critical patent/CN104280026B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Abstract

The present invention relates to a kind of deepwater robot Long baselines Combinated navigation method based on AUKF, obtains the initial absolute position of deepwater robot as the initial point of reckoning by the use of global positioning system, and adopt the collection initial information of deepwater robot；Build uncented Kalman filter senior filter and the initial information to collecting is filtered estimation, build colourless Kalman's extension filter, estimation is further filtered to information after senior filter filtering estimation, data fusion is carried out to the initial information collected using the method for adaptive uncented Kalman filter, draws the information after fusion.The present invention improves the navigation accuracy of the deepwater robot using Long baselines alignment system, at the same can smoothly under the course needed for deepwater robot control system, depth and carrier coordinate system velocity information.

Description

Deepwater robot Long baselines Combinated navigation method based on AUKF
Technical field
It is specifically a kind of based on adaptive the present invention relates to a kind of deepwater robot airmanship of ocean engineering field Answer the Long baselines Combinated navigation method of uncented Kalman filter.
Background technology
Accurate homing capability is the key of underwater robot effective operation and safe retrieving.Navigation system must provide far Being accurately positioned in distance and Long time scale, speed and attitude information.But due to the complexity by underwater environment, machine The influence of the factors such as people's own vol, weight, the energy and disguise, realize that high-precision underwater robot navigation is still one Item difficult task.
Existing underwater navigation method is mainly multi-sensor combined navigation method, and multi-sensor information is filtered. But for the navigation problem of deepwater robot, when system noise information is unknown, if the noise parameter set and reality Noise has big difference, and may result in the Biased estimator even diverging of filtering algorithm.Marine environment is complicated and changeable, it is difficult to be done to noise Go out accurate prior estimate, therefore to design and using the filtering algorithm more suitable for real-time estimating system noise information. Long baselines location data outlier is more, delay is larger, and working state abnormal in some cases, therefore to Long baselines data Correct processing to improve navigation accuracy it is also most important.
The content of the invention
In view of the shortcomings of the prior art, the present invention provides a kind of deepwater robot based on adaptive uncented Kalman filter Combinated navigation method, it is therefore intended that improve the navigation accuracy of the deepwater robot using Long baselines alignment system, while can put down The velocity information under course, depth and carrier coordinate system needed for sliding deepwater robot control system.
The used to achieve the above object technical scheme of the present invention is：A kind of deepwater robot Long baselines based on AUKF Combinated navigation method, the initial absolute position that deepwater robot is obtained by the use of global positioning system are used as the initial of reckoning Point, and gather the initial information of deepwater robot；
Build uncented Kalman filter senior filter and the initial information to collecting is filtered estimation, build no colour atla Germania extension filter, estimation is further filtered to information after senior filter filtering estimation；
Data fusion is carried out to the initial information collected using the method for adaptive uncented Kalman filter, draws fusion Information afterwards.
The initial information includes the linear velocity information of Doppler log collection, the posture letter of motion sensor collection Breath, the positional information of Long baselines acoustic positioning device collection and the depth information of depth gauge collection.
The Long baselines acoustic positioning device obtains position letter by voluntarily resolving the method for deepwater robot absolute position Breath.
The method for voluntarily resolving deepwater robot absolute position, step are as follows：
Long baselines data pack protocol parses deepwater robot to the range information of each beacon, judges each bootstrap information Validity, ternary two to the range information of beacon, is established according to each effectively absolute location information of beacon and deepwater robot Equation of n th order n group：
(x in above-mentioned equation groupA,yA,zA),(xB,yB,zB),(xC,yC,zC) be respectively it is each effectively beacon absolute position sit Mark, lA、lB、lCRespectively deepwater robot solves this equation group and obtains (x to the distance of each beacono,yo,zo) it is deepwater robot Absolute location coordinates, obtain two groups of solutions of robot absolute location coordinates, according to current time depth gauge gather depth believe Breath judges effective solution in two groups of solutions, as the east orientation position detection value in measurement vector and north orientation position detection value.
Colourless Kalman's senior filter filters the implementation process estimated：
The amount of being estimated average and variance to last moment random vector obtain discrete after non-linear colourless conversion Sigma points, state equation through senior filter calculate renewal Sigma points, then calculate renewal Sigma points state average and Variance, state average and variance are calculated the state average and variance of step estimation；To a step estimation state average and Variance again through it is colourless conversion and senior filter measurement equation calculate renewal, obtain deepwater robot Observable state average, The estimate of variance and covariance；Read sensor collection observation information, and with the estimation of state average, variance, observer state Value and gain matrix calculate estimation, obtain the estimated value of system average, variance, and newly ceased；According to the new breath of senior filter Calculate observation signal of the measured value as extension filter.
The implementation process that extension filter is filtered estimation to systematic procedure noise is as follows：
Last moment is estimated to obtain the diagonal entry and variance of the system noise variance matrix of extension filter, with auxiliary The state equation of wave filter obtains a step estimate of diagonal entry and variance after calculating renewal to diagonal entry and variance； Variance, state variance and the gain matrix estimated with the measurement equation of extension filter according to a step of senior filter calculate renewal The observation estimated afterwards, the measured value of obtained a step estimate, observation and senior filter is calculated into estimation is The estimated value and variance of the diagonal entry for process-noise variance battle array of uniting, when there is new observation information, continue next controlling cycle Senior filter realizes step.
The state equation of the extension filter is established as follows：
WhereinFor the system noise in extension filter,For system noise variance matrix diagonal element.
The measurement establishing equation of the extension filter is as follows：
Wherein, vdiag () represent a vector, its component be matrix () the elements in a main diagonal, system noise side Poor matrix diagonal elementAs the observation signal of system, Pk|k-1The variance estimated for a step of main filter status vector, Pk For the variance of main filter status vector, KkFor the gain matrix of senior filter,For the observation of estimation.
The measurement equation of the senior filter switches over according to the significant instant of Long baselines location data：
When the observation that Long baselines acoustic positioning device provides is effective, measurement equation is defined as：
yk=xk+Nn
Wherein, y=[uy,vy,wy,ry,ξy,ηy,ζy,ψy]TFor measurement vector, wherein uy、vy、wy、ryIt is preceding respectively to speed Degree, side velocity, vertical velocity and the observed quantity for turning bow angular speed, ξy、ηy、ζy、ψyBe north orientation position, east orientation position, depth and Observed quantity of the bow to angle, NnFor observation noise；
When the observation that Long baselines acoustic positioning device provides is invalid, measurement equation is defined as：
Wherein, y=[uy,vy,wy,ry,ζy,ψy]TFor measurement vector.
At the time of the significant instant is the renewal of Long baselines data and not outlier.
The invention has the advantages that and advantage：
1. the present invention is proposed according to Long baselines data pack protocol, the absolute position of robot is resolved by robot to beacon distance The method put, solve the problems, such as that Long baselines alignment system can not provide accurate location information in some cases.
2. the present invention proposes switches observational equation according to the validity of Long baselines location data, solves Long baselines positioning Data outlier is more and the problem of Long baselines acoustic positioning device and inconsistent other sensors gathered data turnover rate.
3. navigation accuracy of the present invention is high, because the extension filter in AUKF algorithms can be carried out to the noise information of system Estimation in real time, and can restrain faster, solve marine environment complicated and changeable and deepwater robot performs task and drawn The system noise risen be difficult to obtain accurate prior information so that cause Biased estimator even filtering divergence the problem of.
Brief description of the drawings
Fig. 1 is the fundamental diagram of the Combinated navigation method of the present invention；
Fig. 2 is the flow chart of the adaptive uncented Kalman filter algorithm of the present invention；
Fig. 3 is the embodiment result of the deepwater robot Long baselines integrated navigation based on AUKF.
Embodiment
Below in conjunction with the accompanying drawings and embodiment the present invention is described in further detail.
A kind of deepwater robot Combinated navigation method based on AUKF, is mainly included the following steps that：
By the use of global positioning system obtain deepwater robot initial absolute location information as reckoning initial point, Linear velocity information is gathered using Doppler log, attitude information is gathered using motion sensor, is positioned using Long baselines acoustics Device and depth gauge collection positional information；Data fusion is carried out to above- mentioned information using adaptive uncented Kalman filter method, The velocity under carrier coordinate system and the position vector under earth coordinates and bow is taken, as filter status vector, to be taken to angle The information of each sensor collection can not provide more accurate as wave filter measurement vector, in some cases Long baselines alignment system Location information when, observation of the robot location as positional information is voluntarily resolved according to beacon distance information, selects long base Line number switches observational equation according to significant instant.Filter result is directly used by deepwater robot control system with navigation system.
Further, if Long baselines alignment system due in water acoustic velocity value set the reason such as inaccuracy to cause position error Larger, the invention further relates to a kind of distance letter of the robot in packet according to Long baselines far from subsea beacon (being no less than 3) The method that breath voluntarily resolves robot absolute position, step are as follows：
At the time of each receiving Long baselines packet, robot is parsed to each beacon according to Long baselines data protocol （Underwater lay should be no less than 3 beacons）Range information, the validity of each bootstrap information is judged by data pack protocol, according to every The individual effectively absolute location information of beacon and robot are to the range information of beacon, the ternary quadratic power that ball corresponding to foundation intersects Journey group, this equation group is solved, obtain two groups of solutions of robot absolute coordinate, sentenced according to the depth information that current time depth gauge gathers Effective solution in fixed two groups of solutions.
It is described as follows using adaptive uncented Kalman filter method and step：
On the basis of deepwater robot kinematics model, velocity under carrier coordinate system is taken, under earth coordinates Position vector and bow to angle as filter status vector, establish system state equation；
The senior filter selection standard UKF wave filters of AUKF methods, to system mode transmission and renewal, estimate in real time and be The status information of system, obtain the end value average of system estimationWith variance Pk；
The new breath information ξ of estimation is calculated according to senior filterkCalculate the measured value Sq of extension filterk；Establish auxiliary filter The state equation and measurement equation of ripple device, according to measured value SqkAs the observation signal of system, using AUKF extension filter KF is estimated the noise information of system in real time, obtains system noise covariance battle array Q diagonal entryAnd variance's Estimated value, noise information here include process noise vkWith measurement noise nk, their covariance matrix is respectively QvAnd Qn。
Further, due to Long baselines acoustic positioning device and Doppler log, motion sensor, depth gauge data Turnover rate is inconsistent, and the outlier of Long baselines acoustic positioning device is more, selects Long baselines data significant instant switching observational equation.
It is as shown in Figure 1 the fundamental diagram of Combinated navigation method of the invention, utilizes Doppler log to gather linear speed Information is spent, using Long baselines acoustic positioning device acquisition plane positional information, using depth gauge sampling depth information, utilizes motion Sensor gathers attitude information, and above- mentioned information is merged using data fusion method, obtains deepwater robot marine 3 D motion trace, while smoothly the course needed for deepwater robot control system and the speed under carrier coordinate system can believe Breath.
(1) take the velocity under carrier coordinate system and the position vector under earth coordinates and bow to angle as wave filter State vector, it is defined as：X=[ux,vx,wx,rx,ξx,ηx,ζx,ψx]T, wherein ux、vx、wxAnd rxIt is forward speed respectively, laterally Speed, vertical velocity and the quantity of state for turning bow angular speed, ξx、ηx、ζxAnd ψxIt is north orientation position, east orientation position, depth and bow respectively To the quantity of state at angle.
State equation is defined as：
NvFor system noise, Δ t is sensor sample interval.
(2) observational equation switches over according to the validity of Long baselines location data, and Long baselines data significant instant refers to At the time of the renewal of Long baselines data and not outlier.
(3) AUKF senior filter is filtered estimation to state equation from UKF.The changing rule of system noise distribution It is unknown, is now generally regarded as the uncorrelated random drift amount of noise driving, therefore selects KF to be filtered as AUKF auxiliary Ripple device.
(4) in AUKF algorithms extension filter state equation structure：
By deepwater robot system noise variance matrix Q diagonal entriesThe state equation of extension filter can be represented For：
WhereinIt is that zero mean Gaussian white noise is zero mean Gaussian white noise for the system noise in extension filter；
Due toChanging rule it is unknown, the incoherent random drift vector of noise driving can be regarded as, therefore will be auxiliary The state equation of wave filter is helped to be defined as：
(5) extension filter measures the structure of equation in AUKF algorithms：
Extension filter is the new breath ξ with senior filterkThe diagonal entry of variance matrixObservation as system is believed Number, the variance P that the step according to senior filter state vector is estimatedk|k-1, the variance P of senior filter state vectork, senior filter Gain matrix KkThe observational equation of composition extension filter is defined as：
Wherein vdiag is a vector for taking the elements in a main diagonal of matrix in bracket to form.
It is illustrated in figure 2 the implementation process of adaptive uncented Kalman filter algorithm.Adaptively uncented Kalman filter is The filtering implementation process that senior filter and extension filter are combined.
The implementation process of senior filter filtering estimation：
To the amount of the being estimated average of last moment random vectorWith variance Pk-1After non-linear colourless conversion, obtain One group of discrete Sigma points χk-1, renewal Sigma points are calculated through system state equation, then calculate the state of renewal Sigma points AverageWith variance Pk|k-1, to state averageWith variance Pk|k-1The state average and variance of step estimation is calculated； State average and variance to step estimation calculate renewal through colourless conversion and with measurement equation again, and it is considerable to obtain deepwater robot The average of survey stateIts varianceAnd covarianceEstimate；Read sensor collection observable information ykSame shape State averageWith variance Pk|k-1With the estimate and gain matrix K of Observable statekIt is computed estimating, obtains system averageVariance PkEstimated value, therefrom obtain newly ceasing ξk；According to the new breath ξ of senior filterkCalculate measured value SqkAs auxiliary The observation signal of wave filter, then continue extension filter program step.
Extension filter KF is filtered the implementation process of estimation to systematic procedure noise：
Last moment is estimated to obtain the diagonal entry of the system noise variance matrix of extension filterWith its varianceWith the state equation pair of extension filterWithDiagonal entry is obtained after calculating renewalWith its variance A step estimate；The variance P estimated with the measurement equation of extension filter according to a step of senior filterk|k-1, state variance Pk With gain matrix KkIt is computed the observation estimated after renewalA resulting step estimate and the observation of estimationWith the measured value Sq of senior filterkIt is computed estimation and obtains systematic procedure noise variance matrix QvDiagonal entry estimated valueWith its variance, when there is new observation information ykWhen, continue next controlling cycle senior filter program step.
Long baselines alignment system can not provide accurate location information in some cases, described using according to Long baselines Range information of the robot far from subsea beacon (being no less than 3) in packet voluntarily resolves the method step of robot absolute position It is rapid as follows：
At the time of each receiving Long baselines packet, robot is parsed to each beacon according to Long baselines data protocol （Underwater lay should be no less than 3 beacons）Range information, the validity of each bootstrap information is judged by data pack protocol, according to every The individual effectively absolute location information of beacon and robot are to the range information of beacon, the ternary quadratic power that ball corresponding to foundation intersects Journey group：
(x in above-mentioned equation groupA,yA,zA),(xB,yB,zB),(xC,yC,zC) for it is each effectively beacon absolute location coordinates, lAlBlCDistance for robot to each beacon, solve this equation group and obtain (xo,yo,zo) be robot absolute location coordinates. To two groups of solutions of robot absolute coordinate, the depth information gathered according to current time depth gauge judges effective in two groups of solutions Solution, this solution is as the east orientation in measurement vector and north orientation position detection value.
Such as the embodiment result that Fig. 3 is the deepwater robot Long baselines integrated navigation based on AUKF：
In outfield experiments, robot is with the speed no more than 1.5m/s in surface navigation.Fig. 3, which gives, utilizes base of the present invention The result of robot navigation is carried out in the deepwater robot Long baselines Combinated navigation method of adaptive uncented Kalman filter, and together The positioning result of Long baselines equipment and the navigation results of dead reckoning are made comparisons.Wherein solid line represents the absolute position that GPS is obtained Put, chain-dotted line represents the navigation results of dead reckoning, and point represents the positioning result of Long baselines equipment, and dotted line is represented based on adaptive Answer the navigation results of the deepwater robot Long baselines Combinated navigation method of uncented Kalman filter.From the graph, it is apparent that The rail of the robot estimated using the deepwater robot Long baselines Combinated navigation method based on adaptive uncented Kalman filter The accumulated error that mark can overcome the positioning of Long baselines equipment in some cases inaccurate and dead reckoning increases with the time, and more The absolute position obtained close to GPS.This embodiment shows the deepwater robot of the invention based on adaptive uncented Kalman filter Long baselines Combinated navigation method is remarkably improved integrated navigation system precision.
Information after fusion includes：Plan position information, depth information, attitude information, linear velocity information, angular speed letter Breath.

Claims (6)

  1. A kind of 1. deepwater robot Long baselines Combinated navigation method based on AUKF, it is characterised in that：
    The initial absolute position of deepwater robot is obtained as the initial point of reckoning by the use of global positioning system, and gathers depth The initial information of extra large robot；
    Build uncented Kalman filter senior filter and the initial information to collecting is filtered estimation, build colourless Kalman Extension filter, estimation is further filtered to information after senior filter filtering estimation；
    Data fusion is carried out to the initial information collected using the method for adaptive uncented Kalman filter, after drawing fusion Information；
    Colourless Kalman's senior filter filters the implementation process estimated：
    The amount of being estimated average and variance to last moment random vector obtains discrete Sigma points after non-linear colourless conversion, State equation through senior filter calculates renewal Sigma points, then calculates the state average and variance of renewal Sigma points, right The state average and variance of step estimation is calculated in state average and variance；The state average and variance of the estimation of one step are passed through again The measurement equation of colourless conversion and senior filter calculates renewal, obtains the average, variance and association of deepwater robot Observable state The estimate of variance；Read sensor collection observation information, and with state average, variance, the estimate of observer state and gain Matrix computations are estimated, obtain the estimated value of system average, variance, and newly ceased；It is computed according to the new breath of senior filter Observation signal to measured value as extension filter；
    The initial information includes the linear velocity information of Doppler log collection, and the attitude information of motion sensor collection is long The positional information of baseline acoustic positioner collection and the depth information of depth gauge collection；
    The Long baselines acoustic positioning device obtains positional information by voluntarily resolving the method for deepwater robot absolute position；
    The method for voluntarily resolving deepwater robot absolute position, step are as follows：
    Long baselines data pack protocol parses deepwater robot to the range information of each beacon, judges the effective of each bootstrap information Property, according to each effectively absolute location information of beacon and deepwater robot to the range information of beacon, establish ternary quadratic power Journey group：
    <mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mi>A</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>A</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>z</mi> <mi>A</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>=</mo> <msubsup> <mi>l</mi> <mi>A</mi> <mn>2</mn> </msubsup> </mrow>
    <mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mi>B</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>B</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>z</mi> <mi>B</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>=</mo> <msubsup> <mi>l</mi> <mi>B</mi> <mn>2</mn> </msubsup> </mrow>
    <mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>x</mi> <mi>C</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>C</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>o</mi> </msub> <mo>-</mo> <msub> <mi>z</mi> <mi>C</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>=</mo> <msubsup> <mi>l</mi> <mi>C</mi> <mn>2</mn> </msubsup> </mrow>
    (x in above-mentioned equation groupA,yA,zA),(xB,yB,zB),(xC,yC,zC) be respectively it is each effectively beacon absolute location coordinates, lA、lB、lCRespectively deepwater robot solves this equation group and obtains (x to the distance of each beacono,yo,zo) it is the exhausted of deepwater robot To position coordinates, two groups of solutions of robot absolute location coordinates are obtained, are sentenced according to the depth information that current time depth gauge gathers Effective solution in fixed two groups of solutions, as the east orientation position detection value in measurement vector and north orientation position detection value.
  2. 2. the deepwater robot Long baselines Combinated navigation method according to claim 1 based on AUKF, it is characterised in that：It is auxiliary Help wave filter systematic procedure noise is filtered estimation implementation process it is as follows：
    Last moment is estimated to obtain the diagonal entry and variance of the system noise variance matrix of extension filter, uses auxiliary filter The state equation of device obtains a step estimate of diagonal entry and variance after calculating renewal to diagonal entry and variance；With auxiliary Variance, state variance and the gain matrix for helping the measurement equation of wave filter to be estimated according to a step of senior filter obtain after calculating renewal To the observation of estimation, the measured value of obtained a step estimate, observation and senior filter is calculated into estimation and obtains system mistake The estimated value and variance of the diagonal entry of journey noise variance matrix, when there is new observation information, continue the main filter of next controlling cycle Ripple device realizes step.
  3. 3. the deepwater robot Long baselines Combinated navigation method according to claim 2 based on AUKF, it is characterised in that：Institute The state equation foundation for stating extension filter is as follows：
    <mrow> <msub> <mover> <mi>q</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>w</mi> <msub> <mi>q</mi> <mi>k</mi> </msub> </msub> </mrow>
    Wherein wqkFor the system noise in extension filter,For system noise variance matrix diagonal element.
  4. 4. the deepwater robot Long baselines Combinated navigation method according to claim 2 based on AUKF, it is characterised in that：Institute The measurement establishing equation for stating extension filter is as follows：
    <mrow> <mover> <mi>S</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mi>q</mi> <mi>k</mi> </msub> <mo>=</mo> <mi>g</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>v</mi> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mo>&amp;lsqb;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>&amp;rsqb;</mo> </mrow>
    Wherein, vdiag () represent a vector, its component be matrix () the elements in a main diagonal, system noise variance square Battle array diagonal elementAs the observation signal of system, Pk|k-1The variance estimated for a step of main filter status vector, PkBased on The variance of filter status vector, KkFor the gain matrix of senior filter,For the observation of estimation.
  5. 5. the deepwater robot Long baselines Combinated navigation method according to claim 1 based on AUKF, it is characterised in that：Institute The measurement equation for stating senior filter switches over according to the significant instant of Long baselines location data：
    When the observation that Long baselines acoustic positioning device provides is effective, measurement equation is defined as：
    <mrow> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>1</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>2</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>2</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>3</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <mi>y</mi> <mn>4</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>4</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>4</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <mi>y</mi> <mn>5</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>5</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>5</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <mi>y</mi> <mn>6</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>6</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>6</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <mi>y</mi> <mn>7</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>7</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>7</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <mi>y</mi> <mn>8</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>8</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>8</mn> <mi>n</mi> </msubsup> </mrow>
    Wherein, y=[uy,vy,wy,ry,ξy,ηy,ζy,ψy]TFor 8 dimension measurement vectors, wherein uy、vy、wy、ryBe respectively forward speed, Side velocity, vertical velocity and the observed quantity for turning bow angular speed, ξy、ηy、ζy、ψyIt is north orientation position, east orientation position, depth and bow Observed quantity to angle, NnFor observation noise；X=[ux,vx,wx,rx,ξx,ηx,ζx,ψx]TFor state vector, wherein ux、vx、wxAnd rx It is forward speed, side velocity, vertical velocity and the quantity of state for turning bow angular speed respectively, ξx、ηx、ζxAnd ψxIt is north orientation position respectively Put, east orientation position, depth and quantity of state from bow to angle；
    When the observation that Long baselines acoustic positioning device provides is invalid, measurement equation is defined as：
    <mrow> <msub> <msup> <mi>y</mi> <mo>&amp;prime;</mo> </msup> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>1</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <msup> <mi>y</mi> <mo>&amp;prime;</mo> </msup> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>2</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>2</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <msup> <mi>y</mi> <mo>&amp;prime;</mo> </msup> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>3</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <msup> <mi>y</mi> <mo>&amp;prime;</mo> </msup> <mn>4</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>4</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>4</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <msup> <mi>y</mi> <mo>&amp;prime;</mo> </msup> <mn>5</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>7</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>5</mn> <mi>n</mi> </msubsup> </mrow>
    <mrow> <msub> <msup> <mi>y</mi> <mo>&amp;prime;</mo> </msup> <mn>6</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mn>8</mn> </msub> <mo>+</mo> <msubsup> <mi>N</mi> <mn>6</mn> <mi>n</mi> </msubsup> </mrow>
    Wherein, y'=[uy,vy,wy,ry,ζy,ψy]TFor 6 dimension measurement vectors.
  6. 6. the deepwater robot Long baselines Combinated navigation method according to claim 5 based on AUKF, it is characterised in that：Institute Significant instant is stated for the renewal of Long baselines data and not at the time of outlier.
CN201310284939.0A 2013-07-08 2013-07-08 Deepwater robot Long baselines Combinated navigation method based on AUKF Active CN104280026B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201310284939.0A CN104280026B (en) 2013-07-08 2013-07-08 Deepwater robot Long baselines Combinated navigation method based on AUKF

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201310284939.0A CN104280026B (en) 2013-07-08 2013-07-08 Deepwater robot Long baselines Combinated navigation method based on AUKF

Publications (2)

Publication Number Publication Date
CN104280026A CN104280026A (en) 2015-01-14
CN104280026B true CN104280026B (en) 2017-11-14

Family

ID=52255154

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201310284939.0A Active CN104280026B (en) 2013-07-08 2013-07-08 Deepwater robot Long baselines Combinated navigation method based on AUKF

Country Status (1)

Country Link
CN (1) CN104280026B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107576939A (en) * 2017-07-21 2018-01-12 哈尔滨工程大学 A kind of single beacon distance-measuring and positioning method based on virtual ranging beacon
CN109376642B (en) * 2018-10-16 2022-03-04 长安大学 Method for predicting state of moving vehicle based on driver behavior
CN109375646A (en) * 2018-11-14 2019-02-22 江苏科技大学 AUV docking recycling autonomous navigation method based on FMSRUPF algorithm
CN110542884B (en) * 2019-08-28 2020-11-06 中国科学院声学研究所 Long baseline navigation positioning method based on inertial navigation correction

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361431A (en) * 2000-12-23 2002-07-31 林清芳 Complete integral navigation positioning method and system
CN1779485A (en) * 2004-11-17 2006-05-31 中国科学院沈阳自动化研究所 Assembled navigation positioning method for manned submersible
CN102519450A (en) * 2011-12-12 2012-06-27 东南大学 Integrated navigation device for underwater glider and navigation method therefor

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361431A (en) * 2000-12-23 2002-07-31 林清芳 Complete integral navigation positioning method and system
CN1779485A (en) * 2004-11-17 2006-05-31 中国科学院沈阳自动化研究所 Assembled navigation positioning method for manned submersible
CN102519450A (en) * 2011-12-12 2012-06-27 东南大学 Integrated navigation device for underwater glider and navigation method therefor

Also Published As

Publication number Publication date
CN104280026A (en) 2015-01-14

Similar Documents

Publication Publication Date Title
CN104280025B (en) Deepwater robot ultra-short baseline Combinated navigation method based on uncented Kalman filter
CN108362281B (en) Long-baseline underwater submarine matching navigation method and system
CN102538781B (en) Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method
CN106772524B (en) A kind of agricultural robot integrated navigation information fusion method based on order filtering
CN106885576B (en) AUV (autonomous Underwater vehicle) track deviation estimation method based on multipoint terrain matching positioning
CN109282808B (en) Unmanned aerial vehicle and multi-sensor fusion positioning method for bridge three-dimensional cruise detection
CN109459019A (en) A kind of vehicle mounted guidance calculation method based on cascade adaptive robust federated filter
CN108490433B (en) Deviation Combined estimator and compensation method and system when sky based on Sequential filter
CN101793522B (en) Steady filtering method based on robust estimation
CN103776453A (en) Combination navigation filtering method of multi-model underwater vehicle
CN104280026B (en) Deepwater robot Long baselines Combinated navigation method based on AUKF
CN107702712A (en) Indoor pedestrian&#39;s combined positioning method based on inertia measurement bilayer WLAN fingerprint bases
CN103968838B (en) Co-location method of AUVs (Autonomous Underwater Vehicles) in curvilinear motion state based on polar coordinate system
CN104197935B (en) Indoor localization method based on mobile intelligent terminal
CN106840211A (en) A kind of SINS Initial Alignment of Large Azimuth Misalignment On methods based on KF and STUPF combined filters
CN107063245A (en) A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF
CN104280024B (en) Device and method for integrated navigation of deepwater robot
CN103399336A (en) GPS/SINS (global positioning system/strapdown inertial navigation system) combined navigation method under non-Gauss noise environment
CN105737850B (en) Mutative scale one direction gravity sample vector matching locating method based on particle filter
CN111174774A (en) Navigation information fusion method and system under water level mode at certain depth
CN113433553A (en) Precise navigation method for multi-source acoustic information fusion of underwater robot
CN109813316B (en) Terrain-assisted underwater carrier tight combination navigation method
CN109387198A (en) A kind of inertia based on sequential detection/visual odometry Combinated navigation method
CN109741372A (en) A kind of odometer method for estimating based on binocular vision
Wei et al. Unscented information filter based multi-sensor data fusion using stereo camera, laser range finder and GPS receiver for vehicle localization

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant