CN109947116A - The localization method and device of pilotless automobile - Google Patents
The localization method and device of pilotless automobile Download PDFInfo
- Publication number
- CN109947116A CN109947116A CN201910314074.5A CN201910314074A CN109947116A CN 109947116 A CN109947116 A CN 109947116A CN 201910314074 A CN201910314074 A CN 201910314074A CN 109947116 A CN109947116 A CN 109947116A
- Authority
- CN
- China
- Prior art keywords
- current
- positioning result
- posture
- matrix
- observation
- 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.)
- Granted
Links
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
The present invention discloses the localization method and device of a kind of pilotless automobile, is related to unmanned technical field, accurately positions to the pose of pilotless automobile.The present invention includes: to obtain current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix, and according to current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix, calculates time update position and posture and the time updates covariance matrix；Obtain Current observation positioning result；Current observation matrix and Current observation covariance matrix are obtained, and Current observation matrix, Current observation covariance matrix and time are updated into covariance matrix and substitute into the first preset algorithm, calculates current kalman gain matrix；Position and posture, current kalman gain matrix and Current observation positioning result will be updated the time and substitute into the second preset algorithm, calculate current predictive position and posture.The present invention is suitable for positioning the pose of pilotless automobile.
Description
Technical field
The present invention relates to unmanned technical fields, more particularly to the localization method and dress of a kind of pilotless automobile
It sets.
Background technique
With unmanned technology continuous development and become increasingly popular, more and more closings or semiclosed scene start to adopt
Support is assisted operation process and provided with unmanned technology, and harbour environment is one of them.In unmanned vapour
Vehicle carry out operation during, pilotless automobile needs to obtain the positioning result of itself in real time, thus guarantee inherently safe and
Therefore how job stability accurately positions the pose of pilotless automobile, to obtain pilotless automobile
Positioning result is vital.
Currently, pilotless automobile would generally be fixed by differential GPS positioning mode, laser positioning method and three kinds of vision positioning method
Any one localization method in the method for position obtains the positioning result of pilotless automobile, however, since harbour environment is more multiple
Track crane and container change in location in miscellaneous and harbour environment is frequent, so that existing localization method can not be accurately right
The pose of pilotless automobile is positioned, and then pilotless automobile is caused to be determined by what existing localization method acquired
The accuracy of position result is lower.
Summary of the invention
In view of this, the present invention provides the localization method and device of a kind of pilotless automobile, main purpose is precisely
The pose of pilotless automobile is positioned, to acquire the higher positioning result of accuracy.
In order to achieve the above object, present invention generally provides following technical solutions:
In a first aspect, the present invention provides a kind of localization methods of pilotless automobile, this method comprises:
Current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix are obtained,
And according to the current control amount, the history observation bit appearance state, the historical forecast position and posture and the historical forecast
Covariance matrix, calculates time update position and posture and the time updates covariance matrix；
Obtain Current observation positioning result；
Obtain Current observation matrix and Current observation covariance matrix, and by the Current observation matrix, the current sight
It surveys covariance matrix and the time updates covariance matrix and substitutes into the first preset algorithm, calculate the current kalman gain
Matrix；
The time is updated into position and posture, the current kalman gain matrix and the Current observation positioning result generation
Enter in the second preset algorithm, calculates current predictive position and posture.
Optionally, described according to the current control amount, the history observation bit appearance state, the historical forecast pose shape
State and the historical forecast covariance matrix, calculate time update position and posture and the time updates covariance matrix, comprising:
Local derviation processing is carried out to the history observation bit appearance state using preset function, to obtain the history observation bit appearance
Corresponding first Jacobian matrix of state；
Local derviation processing is carried out to the current control amount using the preset function, it is corresponding to obtain the current control amount
The second Jacobian matrix；
By first Jacobian matrix, second Jacobian matrix, the current control amount and the historical forecast
Position and posture substitutes into third preset algorithm, calculates the time update position and posture；
By first Jacobian matrix, second Jacobian matrix, the historical forecast covariance matrix and make by oneself
Adopted algebraic quantity substitutes into the 4th preset algorithm, calculates the time update covariance matrix.
Optionally, the acquisition Current observation positioning result, comprising:
The time is updated position and posture to substitute into the 5th preset algorithm, calculates current predictive positioning result；
The first current positioning result is obtained, and calculates the described first current positioning result and the current predictive positioning result
Between the first difference；
If first difference is less than or equal to preset threshold vector, the described first current positioning result is determined as institute
State Current observation positioning result；
If first difference is greater than the preset threshold vector, the second current positioning result is obtained, and described in calculating
The second difference between second current positioning result and the current predictive positioning result；
If second difference is less than or equal to the preset threshold vector, the described second current positioning result is determined
For the Current observation positioning result；
If second difference is greater than the preset threshold vector, the current positioning result of third is obtained, and described in calculating
Third difference between the current positioning result of third and the current predictive positioning result；
If the third difference is less than or equal to the preset threshold vector, the current positioning result of the third is determined
For the Current observation positioning result；
If the third difference is greater than the preset threshold vector, and first difference, the second difference and third is poor
The corresponding current positioning result of minimum value in value is determined as the Current observation positioning result；Wherein, described first is current fixed
Position result is first positioning result acquired based on differential GPS positioning mode, laser positioning method and vision positioning method, described
Second current positioning result is that second acquired based on differential GPS positioning mode, laser positioning method and vision positioning method is determined
Position is as a result, the current positioning result of the third is to be acquired based on differential GPS positioning mode, laser positioning method and vision positioning method
Third positioning result.
Optionally, the time is updated into position and posture, the current kalman gain matrix and described current described
Positioning result is observed to substitute into the second preset algorithm, after calculating current predictive position and posture, the method also includes:
The Current observation matrix, the current kalman gain matrix and the time are updated covariance matrix to substitute into
In 6th preset algorithm, current predictive covariance matrix is calculated；
The current predictive position and posture, the current predictive covariance matrix and the Current observation positioning result are delayed
It deposits into local storage space.
Optionally, the time is updated into position and posture, the current kalman gain matrix and described current described
Positioning result is observed to substitute into the second preset algorithm, after calculating current predictive position and posture, the method also includes:
The current predictive position and posture is carried out output to show.
Second aspect, the present invention also provides a kind of positioning device of pilotless automobile, which includes:
First acquisition unit, for obtaining current control amount, history observation bit appearance state, historical forecast position and posture and going through
History predicts covariance matrix；
First computing unit, for according to the current control amount, the history observation bit appearance state, the historical forecast
Position and posture and the historical forecast covariance matrix, calculate time update position and posture and the time updates covariance matrix；
Second acquisition unit, for obtaining Current observation positioning result；
Third acquiring unit, for obtaining Current observation matrix and Current observation covariance matrix；
Second computing unit was used for the Current observation matrix, the Current observation covariance matrix and the time
It updates covariance matrix to substitute into the first preset algorithm, calculates the current kalman gain matrix；
Third computing unit, for the time to be updated position and posture, the current kalman gain matrix and described
Current observation positioning result substitutes into the second preset algorithm, calculates current predictive position and posture.
Optionally, first computing unit includes:
First local derviation module, for carrying out local derviation processing to the history observation bit appearance state using preset function, to obtain
Obtain corresponding first Jacobian matrix of the history observation bit appearance state；
Second local derviation module, for carrying out local derviation processing to the current control amount using the preset function, to obtain
Corresponding second Jacobian matrix of the current control amount；
First computing module is used for first Jacobian matrix, second Jacobian matrix, the current control
Amount and the historical forecast position and posture substitute into third preset algorithm, calculate the time update position and posture；
Second computing module is used for first Jacobian matrix, second Jacobian matrix, the historical forecast
Covariance matrix and customized algebraic quantity substitute into the 4th preset algorithm, calculate the time update covariance matrix.
Optionally, the second acquisition unit includes:
Third computing module substitutes into the 5th preset algorithm for the time to be updated position and posture, calculates current pre-
Survey positioning result；
4th computing module for obtaining the first current positioning result, and calculates the described first current positioning result and institute
State the first difference between current predictive positioning result；
First determining module, for when first difference is less than or equal to preset threshold vector, described first to be worked as
Prelocalization result is determined as the Current observation positioning result；
5th computing module, for it is currently fixed to obtain second when first difference is greater than the preset threshold vector
Position is as a result, and calculate the second difference between the described second current positioning result and the current predictive positioning result；
Second determining module, for when second difference is less than or equal to the preset threshold vector, by described the
Two current positioning results are determined as the Current observation positioning result；
6th computing module, for it is currently fixed to obtain third when second difference is greater than the preset threshold vector
Position is as a result, and calculate the third difference between the current positioning result of the third and the current predictive positioning result；
Third determining module, for when the third difference is less than or equal to the preset threshold vector, by described the
Three current positioning results are determined as the Current observation positioning result；
4th determining module, for when the third difference be greater than the preset threshold vector when, by first difference,
The corresponding current positioning result of minimum value in second difference and third difference is determined as the Current observation positioning result；Its
In, the first current positioning result is the acquired based on differential GPS positioning mode, laser positioning method and vision positioning method
One positioning result, the second current positioning result are to be obtained based on differential GPS positioning mode, laser positioning method and vision positioning method
Second positioning result obtained, the current positioning result of third are based on differential GPS positioning mode, laser positioning method and view
Feel the third positioning result that positioning mode acquires.
Optionally, described device further include:
4th computing unit, for the time to be updated position and posture, the current card in the third computing unit
Germania gain matrix and the Current observation positioning result substitute into the second preset algorithm, calculate current predictive position and posture it
Afterwards, the Current observation matrix, the current kalman gain matrix and the time are updated into covariance matrix and substitutes into the 6th
In preset algorithm, current predictive covariance matrix is calculated；
Cache unit, for by the current predictive position and posture, the current predictive covariance matrix and described current
Observation positioning result is cached into local storage space.
Optionally, described device further include:
Output unit, for the time to be updated position and posture, the current Kalman in the third computing unit
Gain matrix and the Current observation positioning result substitute into the second preset algorithm, will after calculating current predictive position and posture
The current predictive position and posture carries out output and shows.
To achieve the goals above, according to the third aspect of the invention we, a kind of storage medium, the storage medium are provided
Program including storage, wherein equipment where controlling the storage medium in described program operation executes nothing described above
The localization method of people's driving.
To achieve the goals above, according to the fourth aspect of the invention, a kind of processor is provided, the processor is used for
Run program, wherein described program executes the localization method of pilotless automobile described above when running.
By above-mentioned technical proposal, technical solution provided by the invention is at least had the advantage that
The localization method and device of a kind of pilotless automobile provided by the invention, with pilotless automobile in the prior art
It is obtained based on any one localization method in three kinds of differential GPS positioning mode, laser positioning method and vision positioning method localization methods
Itself corresponding positioning result is compared, and the present invention can make pilotless automobile first obtain itself corresponding current control amount, go through
History observes position and posture, historical forecast position and posture and historical forecast covariance matrix, further according to acquisition current control amount, go through
History observes position and posture, historical forecast position and posture and historical forecast covariance matrix, calculates itself corresponding time update position
Appearance state and time update covariance matrix, and after acquiring itself corresponding Current observation positioning result, obtain itself
Corresponding Current observation matrix and Current observation covariance matrix, and according to the Current observation matrix of acquisition, Current observation association side
Poor matrix and time update covariance matrix, calculate itself corresponding current kalman gain matrix, and obtain according to calculating
Current kalman gain matrix, the time update position and posture and Current observation positioning result, it is corresponding current pre- to calculate itself
Survey position and posture.Since pilotless automobile is based on itself corresponding Current observation positioning result, current kalman gain square
Battle array and time update position and posture, calculate itself corresponding current predictive position and posture, that is, are based on EKF (Extended
Kalman Filter, extended Kalman filter) frame prediction current time itself corresponding positioning result, therefore, nobody drives
Sail automobile also can accurately position the pose of itself in complicated harbour environment, unmanned so as to guarantee
Safety and job stability of the automobile in operation process.
The above description is only an overview of the technical scheme of the present invention, in order to better understand the technical means of the present invention,
And it can be implemented in accordance with the contents of the specification, and in order to allow above and other objects of the present invention, feature and advantage can
It is clearer and more comprehensible, the followings are specific embodiments of the present invention.
Detailed description of the invention
By reading the following detailed description of the preferred embodiment, various other advantages and benefits are common for this field
Technical staff will become clear.The drawings are only for the purpose of illustrating a preferred embodiment, and is not considered as to the present invention
Limitation.And throughout the drawings, the same reference numbers will be used to refer to the same parts.In the accompanying drawings:
Fig. 1 shows a kind of localization method flow chart of pilotless automobile provided in an embodiment of the present invention；
Fig. 2 shows the localization method flow charts of another pilotless automobile provided in an embodiment of the present invention；
Fig. 3 shows a kind of composition block diagram of the positioning device of pilotless automobile provided in an embodiment of the present invention；
Fig. 4 shows the composition block diagram of the positioning device of another pilotless automobile provided in an embodiment of the present invention.
Specific embodiment
Exemplary embodiments of the present disclosure are described in more detail below with reference to accompanying drawings.Although showing the disclosure in attached drawing
Exemplary embodiment, it being understood, however, that may be realized in various forms the disclosure without should be by embodiments set forth here
It is limited.On the contrary, these embodiments are provided to facilitate a more thoroughly understanding of the present invention, and can be by the scope of the present disclosure
It is fully disclosed to those skilled in the art.
The embodiment of the present invention provides a kind of localization method of pilotless automobile, as shown in Figure 1, this method comprises:
101, current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance are obtained
Matrix, and according to current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix,
It calculates time update position and posture and the time updates covariance matrix.
Wherein, the corresponding current control amount of pilotless automobile is last moment to current time pilotless automobile angle speed
The changing value of degree and acceleration, specifically, pilotless automobile can pass through Inertial Measurement Unit (Inertial
Measurement unit, IMU) obtain itself corresponding current control amount；Wherein, the corresponding observation positioning of pilotless automobile
It as a result is the positioning result acquired based on existing localization method, it includes the corresponding direction of pilotless automobile and positions
It sets, the corresponding observation position and posture of pilotless automobile is to carry out the positioning result that conversion process obtains to observation positioning result,
It includes the corresponding direction of pilotless automobile, position and speed, thus the corresponding history observation bit appearance shape of pilotless automobile
State is the corresponding observation position and posture of last moment pilotless automobile, and it includes the corresponding courts of last moment pilotless automobile
To, position and speed；Wherein, the corresponding prediction position and posture of pilotless automobile is to carry out prediction pilotless automobile pose shape
During state operates, the positioning result being calculated, it includes the corresponding direction of pilotless automobile, position and speed, from
And the corresponding historical forecast position and posture of pilotless automobile is the corresponding prediction position and posture of last moment pilotless automobile；
Wherein, the corresponding prediction covariance matrix of pilotless automobile is the process for carrying out prediction pilotless automobile position and posture operation
In, the covariance matrix being calculated, thus the corresponding historical forecast covariance matrix of pilotless automobile be last moment without
The corresponding prediction covariance matrix of people's driving.
In embodiments of the present invention, pilotless automobile is acquiring an observation positioning based on existing localization method
As a result, and after being initialized based on the observation positioning result to the position and posture of itself, itself current time pair can be based on
The observation positioning result (i.e. Current observation positioning result) answered calculates current time itself corresponding prediction position and posture and (works as
Preceding prediction position and posture).And pilotless automobile is being based on itself corresponding Current observation positioning result, and it is corresponding to calculate itself
When current predictive position and posture, it is necessary first to obtain itself corresponding current control amount, history observation bit appearance state, historical forecast
Position and posture and historical forecast covariance matrix, and time update processing is carried out to the historical forecast position and posture acquired,
Position and posture is updated to obtain itself corresponding time, and the time is carried out to the historical forecast covariance matrix acquired
Update processing, so that obtaining itself corresponding time updates covariance matrix, i.e., according to current control amount, the history acquired
Position and posture, historical forecast position and posture and historical forecast covariance matrix are observed, itself corresponding time update pose is calculated
State and time update covariance matrix, update pose based on itself corresponding Current observation positioning result, time so as to subsequent
State and time update covariance matrix, calculate itself corresponding current predictive position and posture.
102, Current observation positioning result is obtained.
In embodiments of the present invention, pilotless automobile is according to itself corresponding current control amount, history observation bit appearance
Itself corresponding time update position and posture is calculated in state, historical forecast position and posture and historical forecast covariance matrix
And after the time updates covariance matrix, itself corresponding Current observation positioning result can be obtained, so as to subsequent right based on itself
The Current observation positioning result answered calculates itself corresponding current predictive position and posture.Specifically, in embodiments of the present invention,
Pilotless automobile can acquire current time itself corresponding positioning knot based on any one existing localization method
After fruit, which is determined as itself corresponding current predictive position and posture, every kind of existing positioning can also be based on
After method acquires current time itself corresponding positioning result, it is corresponding current that optimal positioning result is determined as itself
Predict position and posture, but not limited to this.
103, Current observation matrix and Current observation covariance matrix are obtained, and Current observation matrix, Current observation are assisted
Variance matrix and time update covariance matrix and substitute into the first preset algorithm, calculate current kalman gain matrix.
In embodiments of the present invention, pilotless automobile is after acquiring itself corresponding Current observation positioning result,
Current time itself corresponding observing matrix and observation covariance matrix (i.e. Current observation matrix and Current observation association can be obtained
Variance matrix), and will acquire Current observation matrix, Current observation covariance matrix and be calculated in a step 101
Time update covariance matrix substitute into the first preset algorithm in, to calculate current time itself corresponding kalman gain square
Battle array (i.e. current kalman gain matrix), wherein the first preset algorithm is specific as follows:
Kk=Pk|k-1Hk T(Hk Pk|k-1Hk T+Rk)-1
Wherein, KkFor current kalman gain matrix, Pk|k-1Covariance matrix, H are updated for the timekFor Current observation square
Battle array, RkFor Current observation covariance matrix.
104, position and posture, current kalman gain matrix and Current observation positioning result will be updated the time and substitute into second in advance
In imputation method, current predictive position and posture is calculated.
In embodiments of the present invention, itself corresponding current karr is being calculated by step 103 in pilotless automobile
After graceful gain matrix, the time being calculated in a step 101 can be updated to position and posture, be calculated in step 103
Current kalman gain matrix and the Current observation positioning result that acquires in a step 102 substitute into the second preset algorithm
In, to calculate current time itself corresponding prediction position and posture (i.e. current predictive position and posture), it is based on EKF
(Extended Kalman Filter, extended Kalman filter) frame predicts current time itself corresponding positioning result,
Wherein, the second preset algorithm is specific as follows:
Xk|k=Xk|k-1+Kk[Zk-h(Xk|k-1)]
Wherein, Xk|kFor current predictive position and posture, Xk|k-1Position and posture, K are updated for the timekFor current kalman gain
Matrix, ZkFor Current observation positioning result, h is unit matrix.
The localization method of a kind of pilotless automobile provided in an embodiment of the present invention, with pilotless automobile in the prior art
It is obtained based on any one localization method in three kinds of differential GPS positioning mode, laser positioning method and vision positioning method localization methods
Itself corresponding positioning result is compared, and the embodiment of the present invention can make pilotless automobile first obtain itself corresponding current control
Amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix, further according to the current control of acquisition
Amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix, calculate itself corresponding time more
New position and posture and time update covariance matrix, and after acquiring itself corresponding Current observation positioning result, obtain
Itself corresponding Current observation matrix and Current observation covariance matrix, and according to Current observation matrix, the Current observation of acquisition
Covariance matrix and time update covariance matrix, calculate itself corresponding current kalman gain matrix, and according to calculating
The current kalman gain matrix that obtains, time update position and posture and Current observation positioning result, calculate itself and corresponding work as
Preceding prediction position and posture.Since pilotless automobile is increased based on itself corresponding Current observation positioning result, current Kalman
Beneficial matrix and time update position and posture, calculate itself corresponding current predictive position and posture, that is, are based on EKF (Extended
Kalman Filter, extended Kalman filter) frame prediction current time itself corresponding positioning result, therefore, nobody drives
Sail automobile also can accurately position the pose of itself in complicated harbour environment, unmanned so as to guarantee
Safety and job stability of the automobile in operation process.
Below in order to be explained in more detail, the embodiment of the invention provides the positioning sides of another pilotless automobile
Method, especially pilotless automobile are pre- according to current control amount, history observation bit appearance state, historical forecast position and posture and history
Covariance matrix is surveyed, the time updates position and posture and the time updates covariance matrix specific method and unmanned vapour are calculated
Vehicle obtains the specific method of Current observation positioning result, it is specific as shown in Fig. 2, this method comprises:
201, current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance are obtained
Matrix.
Wherein, about step 201, obtain current control amount, history observation bit appearance state, historical forecast position and posture and go through
History predicts covariance matrix, can refer to the description of Fig. 1 corresponding part, and the embodiment of the present invention will not be described in great detail herein.
202, according to current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance
Matrix, calculates time update position and posture and the time updates covariance matrix.
In embodiments of the present invention, pilotless automobile is obtaining itself corresponding current control amount, history observation bit appearance
It, can be to the historical forecast position and posture acquired after state, historical forecast position and posture and historical forecast covariance matrix
Time update processing is carried out, so that obtaining itself corresponding time updates position and posture, and to the historical forecast acquired
Covariance matrix carries out time update processing, so that obtaining itself corresponding time updates covariance matrix (i.e. according to obtaining
Current control amount, history observation bit appearance state, historical forecast position and posture and the historical forecast covariance matrix arrived, calculates itself
The corresponding time updates position and posture and the time updates covariance matrix).It below will be to pilotless automobile how according to current
Control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix calculate time update pose
State and time update covariance matrix and are described in detail.
(1) local derviation processing is carried out to history observation bit appearance state using preset function, to obtain history observation bit appearance state
Corresponding first Jacobian matrix, and local derviation processing is carried out to current control amount using preset function, to obtain current control
Measure corresponding second Jacobian matrix.
In embodiments of the present invention, pilotless automobile is obtaining itself corresponding current control amount, history observation bit appearance
After state, historical forecast position and posture and historical forecast covariance matrix, what can be acquired respectively using preset function is gone through
History observes position and posture and current control amount and carries out local derviation processing, thus obtain history observation bit appearance state it is corresponding first it is refined can
The second Jacobian matrix more corresponding than matrix and current control amount.
(2) the first Jacobian matrix, the second Jacobian matrix, current control amount and historical forecast position and posture are substituted into the
In three preset algorithms, time update position and posture is calculated.
In embodiments of the present invention, pilotless automobile acquire history observation bit appearance state it is corresponding first it is refined can
After the second Jacobian matrix more corresponding than matrix and current control amount, the first Jacobian matrix and the second Jacobean matrix can be based on
Battle array carries out time update processing to historical forecast position and posture, so that obtaining itself corresponding time updates position and posture, i.e., will
First Jacobian matrix, the second Jacobian matrix, current control amount and historical forecast position and posture substitute into third preset algorithm,
Position and posture is updated to calculate the time, wherein third preset algorithm is specific as follows:
Xk|k-1=AXk-1|k-1+Buk
Wherein, Xk|k-1Position and posture is updated for the time, A is corresponding first Jacobian matrix of history observation bit appearance state,
Xk-1|k-1For historical forecast position and posture, B is corresponding second Jacobian matrix of current control amount, ukFor current control amount.
(3) by the first Jacobian matrix, the second Jacobian matrix, historical forecast covariance matrix and customized algebraic quantity generation
Enter in the 4th preset algorithm, calculates time update covariance matrix.
Wherein, customized algebraic quantity is self-defining, the corresponding algebraic quantity of last moment pilotless automobile.
In embodiments of the present invention, pilotless automobile is according to the first Jacobian matrix, the second Jacobian matrix, current
Control amount and historical forecast position and posture can be refined based on first after itself corresponding time update position and posture is calculated
Time update processing is carried out to historical forecast covariance matrix than matrix and the second Jacobian matrix, is corresponded to obtain itself
Time update covariance matrix, i.e., by the first Jacobian matrix, the second Jacobian matrix, historical forecast covariance matrix and from
It defines algebraic quantity to substitute into the 4th preset algorithm, so that calculating the time updates covariance matrix, wherein the 4th preset algorithm is specific
It is as follows:
Pk|k-1=A Pk-1|k-1AT+BQk-1BT
Wherein, Pk|k-1Covariance matrix is updated for the time, A is corresponding first Jacobean matrix of history observation bit appearance state
Battle array, Pk-1|k-1For historical forecast covariance matrix, B is corresponding second Jacobian matrix of current control amount, Qk-1For customized generation
Quantity.
203, Current observation positioning result is obtained.
In embodiments of the present invention, pilotless automobile is according to itself corresponding current control amount, history observation bit appearance
Itself corresponding time update position and posture is calculated in state, historical forecast position and posture and historical forecast covariance matrix
And after the time updates covariance matrix, itself corresponding Current observation positioning result can be obtained, so as to subsequent right based on itself
The Current observation positioning result answered calculates itself corresponding current predictive position and posture.Below will to pilotless automobile how
Current observation positioning result is obtained to be described in detail.
(1) pilotless automobile will update position and posture the time and substitute into the 5th preset algorithm, to calculate current predictive
Positioning result, wherein the 5th preset algorithm is specific as follows:
Zk|k-1=h (Xk|k-1)
Wherein, Zk|k-1For current predictive positioning result, Xk|k-1Position and posture is updated for the time, h is unit matrix.
(2) pilotless automobile obtains the first current positioning result, and calculates the first current positioning result and current predictive
The first difference between positioning result, i.e. the first difference=the first current positioning result-current predictive positioning result.
(3) when pilotless automobile determines that the first difference is less than or equal to preset threshold vector, pilotless automobile will
First current positioning result is determined as Current observation positioning result.
(4) when pilotless automobile determines that the first difference is greater than preset threshold vector, pilotless automobile obtains second
Current positioning result, and calculate the second difference between the second current positioning result and current predictive positioning result, i.e., second is poor
It is worth the=the second current positioning result-current predictive positioning result.
(5) when pilotless automobile determines that the second difference is less than or equal to preset threshold vector, pilotless automobile will
Second current positioning result is determined as Current observation positioning result.
(6) when pilotless automobile determines that the second difference is greater than preset threshold vector, pilotless automobile obtains third
Current positioning result, and calculate the third difference between the current positioning result of third and current predictive positioning result, i.e. third is poor
The current positioning result of value=third-current predictive positioning result.
(7) when pilotless automobile determines that third difference is less than or equal to preset threshold vector, pilotless automobile will
The current positioning result of third is determined as Current observation positioning result.
(8) when pilotless automobile determines that third difference is greater than preset threshold vector, pilotless automobile is poor by first
The corresponding current positioning result of minimum value in value, the second difference and third difference is determined as Current observation positioning result.
It needs to be illustrated, the first current positioning result is that current time pilotless automobile is fixed based on differential GPS
First positioning result that position method, laser positioning method and vision positioning method acquire, when the second current positioning result is current
Carve second positioning knot that pilotless automobile is acquired based on differential GPS positioning mode, laser positioning method and vision positioning method
Fruit, the current positioning result of third are that current time pilotless automobile is fixed based on differential GPS positioning mode, laser positioning method and vision
The third positioning result that position method acquires obtains for example, current time pilotless automobile is based on differential GPS positioning mode
Second positioning result is acquired to first positioning result, based on laser positioning method, view-based access control model positioning mode acquires
Third positioning result is then the first current positioning result based on the positioning result that differential GPS positioning mode acquires, based on sharp
The positioning result that light-seeking method acquires is the second current positioning result, the positioning result that view-based access control model positioning mode acquires
For the current positioning result of third.
204, Current observation matrix and Current observation covariance matrix are obtained, and Current observation matrix, Current observation are assisted
Variance matrix and time update covariance matrix and substitute into the first preset algorithm, calculate current kalman gain matrix.
Wherein, about step 204, obtain Current observation matrix and Current observation covariance matrix, and by Current observation square
Battle array, Current observation covariance matrix and time update covariance matrix and substitute into the first preset algorithm, calculate current Kalman and increase
Beneficial matrix, can refer to the description of Fig. 1 corresponding part, and the embodiment of the present invention will not be described in great detail herein.
205, position and posture, current kalman gain matrix and Current observation positioning result will be updated the time and substitute into second in advance
In imputation method, current predictive position and posture is calculated.
Wherein, about step 205, will update the time position and posture, current kalman gain matrix and Current observation positioning
As a result it substitutes into the second preset algorithm, calculates current predictive position and posture, the description of Fig. 1 corresponding part, the present invention can be referred to
Embodiment will not be described in great detail herein.
Further, in embodiments of the present invention, pilotless automobile is updating pose shape according to itself corresponding time
State, current kalman gain matrix and Current observation positioning result, after itself corresponding current predictive position and posture is calculated,
By the Current observation matrix acquired in step 204, in step 204 the current kalman gain square that can be calculated
Battle array and the time being calculated in step 202 update covariance matrix and substitute into the 6th preset algorithm, so that it is right to calculate itself
The current predictive covariance matrix answered, and be calculated by the current predictive covariance matrix being calculated, in step 205
Current predictive position and posture and the Current observation positioning result acquired in step 203 are cached into local storage space,
So that pilotless automobile is during subsequent time carries out prediction pilotless automobile position and posture operation, this is operated
The current predictive covariance matrix of caching as historical forecast covariance matrix, using current predictive position and posture as historical forecast
Position and posture, and using Current observation positioning result as history Current observation positioning result, to calculate subsequent time, nobody is driven
Sail the corresponding prediction position and posture of automobile, wherein the 6th preset algorithm is specific as follows:
Pk|k=(I-Kk Hk)Pk|k-1
Wherein, Pk|kFor current predictive covariance matrix, I is the matrix that element is all 1, KkFor current kalman gain square
Battle array, HkFor Current observation matrix, Pk|k-1Covariance matrix is updated for the time.
206, current predictive position and posture output is carried out to show.
In embodiments of the present invention, pilotless automobile is updating position and posture, current card according to itself corresponding time
Germania gain matrix and Current observation positioning result can will be counted after itself corresponding current predictive position and posture is calculated
Obtained current predictive position and posture carries out output and shows, so that staff knows that current time pilotless automobile is corresponding
Direction, position and speed.
To achieve the goals above, according to another aspect of the present invention, the embodiment of the invention also provides a kind of storage Jie
Matter, the storage medium include the program of storage, wherein equipment where controlling the storage medium in described program operation is held
The localization method of row pilotless automobile described above.
To achieve the goals above, according to another aspect of the present invention, the embodiment of the invention also provides a kind of processor,
The processor is for running program, wherein described program executes the positioning side of pilotless automobile described above when running
Method.
Further, as the realization to method shown in above-mentioned Fig. 1 and Fig. 2, another embodiment of the present invention additionally provides one
The positioning device of kind pilotless automobile.The Installation practice is corresponding with preceding method embodiment, is easy to read, present apparatus reality
It applies example no longer to repeat the detail content in preceding method embodiment one by one, it should be understood that the device in the present embodiment
The full content realized in preceding method embodiment can be corresponded to.The device is applied to accurately to the pose of pilotless automobile
It is positioned, so that the higher positioning result of accuracy is acquired, specifically as shown in figure 3, the device includes:
First acquisition unit 31, for obtain current control amount, history observation bit appearance state, historical forecast position and posture and
Historical forecast covariance matrix；
First computing unit 32, for pre- according to the current control amount, the history observation bit appearance state, the history
Position and posture and the historical forecast covariance matrix are surveyed, time update position and posture is calculated and the time updates covariance matrix；
Second acquisition unit 33, for obtaining Current observation positioning result；
Third acquiring unit 34, for obtaining Current observation matrix and Current observation covariance matrix；
Second computing unit 35, for by the Current observation matrix, the Current observation covariance matrix and it is described when
Between update covariance matrix and substitute into the first preset algorithm, calculate the current kalman gain matrix；
Third computing unit 36, for the time to be updated position and posture, the current kalman gain matrix and institute
It states Current observation positioning result to substitute into the second preset algorithm, calculates current predictive position and posture.
Further, as shown in figure 4, the first computing unit 32 includes:
First local derviation module 321, for carrying out local derviation processing to the history observation bit appearance state using preset function, with
Obtain corresponding first Jacobian matrix of the history observation bit appearance state；
Second local derviation module 322, for carrying out local derviation processing to the current control amount using the preset function, to obtain
Obtain corresponding second Jacobian matrix of the current control amount；
First computing module 323, for will first Jacobian matrix, second Jacobian matrix, it is described currently
Control amount and the historical forecast position and posture substitute into third preset algorithm, calculate the time update position and posture；
Second computing module 324 is used for first Jacobian matrix, second Jacobian matrix, the history
It predicts that covariance matrix and customized algebraic quantity substitute into the 4th preset algorithm, calculates the time update covariance matrix.
Further, as shown in figure 4, second acquisition unit 33 includes:
Third computing module 331 substitutes into the 5th preset algorithm for the time to be updated position and posture, calculates current
Predict positioning result；
4th computing module 332, for obtain the first current positioning result, and calculate the described first current positioning result with
The first difference between the current predictive positioning result；
First determining module 333 is used for when first difference is less than or equal to preset threshold vector, by described first
Current positioning result is determined as the Current observation positioning result；
5th computing module 334, for it is current to obtain second when first difference is greater than the preset threshold vector
Positioning result, and calculate the second difference between the described second current positioning result and the current predictive positioning result；
Second determining module 335 is used for when second difference is less than or equal to the preset threshold vector, will be described
Second current positioning result is determined as the Current observation positioning result；
6th computing module 336, for it is current to obtain third when second difference is greater than the preset threshold vector
Positioning result, and calculate the third difference between the current positioning result of the third and the current predictive positioning result；
Third determining module 337 is used for when the third difference is less than or equal to the preset threshold vector, will be described
The current positioning result of third is determined as the Current observation positioning result；
4th determining module 338 is used for when the third difference is greater than the preset threshold vector, poor by described first
The corresponding current positioning result of minimum value in value, the second difference and third difference is determined as the Current observation positioning result；
Wherein, the described first current positioning result is acquired based on differential GPS positioning mode, laser positioning method and vision positioning method
First positioning result, the second current positioning result are based on differential GPS positioning mode, laser positioning method and vision positioning method
Second positioning result acquired, the current positioning result of third be based on differential GPS positioning mode, laser positioning method and
The third positioning result that vision positioning method acquires.
Further, as shown in figure 4, the device further include:
4th computing unit 37, for the time to be updated position and posture, the current card in third computing unit 36
Germania gain matrix and the Current observation positioning result substitute into the second preset algorithm, calculate current predictive position and posture it
Afterwards, the Current observation matrix, the current kalman gain matrix and the time are updated into covariance matrix and substitutes into the 6th
In preset algorithm, current predictive covariance matrix is calculated；
Cache unit 38, for by the current predictive position and posture, the current predictive covariance matrix and described working as
Preceding observation positioning result is cached into local storage space.
Further, as shown in figure 4, the device further include:
Output unit 39, for the time to be updated position and posture, the current Kalman in third computing unit 36
Gain matrix and the Current observation positioning result substitute into the second preset algorithm, will after calculating current predictive position and posture
The current predictive position and posture carries out output and shows.
The localization method and device of a kind of pilotless automobile provided in an embodiment of the present invention, in the prior art nobody drive
Automobile is sailed based on any one positioning side in three kinds of differential GPS positioning mode, laser positioning method and vision positioning method localization methods
Method obtains itself corresponding positioning result and compares, and the embodiment of the present invention can make pilotless automobile first obtain itself corresponding to work as
Preceding control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix, further according to working as acquisition
Preceding control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix, it is corresponding to calculate itself
Time updates position and posture and the time updates covariance matrix, and is acquiring itself corresponding Current observation positioning result
Afterwards, obtain itself corresponding Current observation matrix and Current observation covariance matrix, and according to the Current observation matrix of acquisition, when
Preceding observation covariance matrix and time update covariance matrix, calculate itself corresponding current kalman gain matrix, Yi Jigen
According to the current kalman gain matrix obtained, time update position and posture and Current observation positioning result is calculated, it is right to calculate itself
The current predictive position and posture answered.Since pilotless automobile is based on itself corresponding Current observation positioning result, current card
Germania gain matrix and time update position and posture, calculate itself corresponding current predictive position and posture, that is, are based on EKF
(Extended Kalman Filter, extended Kalman filter) frame predicts current time itself corresponding positioning result,
Therefore, pilotless automobile also can accurately position the pose of itself in complicated harbour environment, so as to
Guarantee safety and job stability of the pilotless automobile in operation process.
The positioning device of the pilotless automobile includes processor and memory, above-mentioned first acquisition unit, the first meter
Unit, second acquisition unit, third acquiring unit, the second computing unit and third computing unit etc. is calculated to deposit as program unit
Storage in memory, executes above procedure unit stored in memory by processor to realize corresponding function.
Include kernel in processor, is gone in memory to transfer corresponding program unit by kernel.Kernel can be set one
Or more, accurately the pose of pilotless automobile is positioned by adjusting kernel parameter, thus acquire it is accurate
The higher positioning result of property.
Memory may include the non-volatile memory in computer-readable medium, random access memory (RAM) and/
Or the forms such as Nonvolatile memory, if read-only memory (ROM) or flash memory (flash RAM), memory include that at least one is deposited
Store up chip.
The embodiment of the invention provides a kind of storage mediums, are stored thereon with program, real when which is executed by processor
The localization method of pilotless automobile described in any one of existing above embodiments.
The embodiment of the invention provides a kind of processor, the processor is for running program, wherein described program operation
The localization method of pilotless automobile described in any one of Shi Zhihang above embodiments.
The embodiment of the invention provides a kind of equipment, equipment include processor, memory and storage on a memory and can
The program run on a processor, processor perform the steps of when executing program
Current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix are obtained,
And according to the current control amount, the history observation bit appearance state, the historical forecast position and posture and the historical forecast
Covariance matrix, calculates time update position and posture and the time updates covariance matrix；
Obtain Current observation positioning result；
Obtain Current observation matrix and Current observation covariance matrix, and by the Current observation matrix, the current sight
It surveys covariance matrix and the time updates covariance matrix and substitutes into the first preset algorithm, calculate the current kalman gain
Matrix；
The time is updated into position and posture, the current kalman gain matrix and the Current observation positioning result generation
Enter in the second preset algorithm, calculates current predictive position and posture.
Further, described according to the current control amount, the history observation bit appearance state, the historical forecast pose
State and the historical forecast covariance matrix, calculate time update position and posture and the time updates covariance matrix, comprising:
Local derviation processing is carried out to the history observation bit appearance state using preset function, to obtain the history observation bit appearance
Corresponding first Jacobian matrix of state；
Local derviation processing is carried out to the current control amount using the preset function, it is corresponding to obtain the current control amount
The second Jacobian matrix；
By first Jacobian matrix, second Jacobian matrix, the current control amount and the historical forecast
Position and posture substitutes into third preset algorithm, calculates the time update position and posture；
By first Jacobian matrix, second Jacobian matrix, the historical forecast covariance matrix and make by oneself
Adopted algebraic quantity substitutes into the 4th preset algorithm, calculates the time update covariance matrix.
Further, the acquisition Current observation positioning result, comprising:
The time is updated position and posture to substitute into the 5th preset algorithm, calculates current predictive positioning result；
The first current positioning result is obtained, and calculates the described first current positioning result and the current predictive positioning result
Between the first difference；
If first difference is less than or equal to preset threshold vector, the described first current positioning result is determined as institute
State Current observation positioning result；
If first difference is greater than the preset threshold vector, the second current positioning result is obtained, and described in calculating
The second difference between second current positioning result and the current predictive positioning result；
If second difference is less than or equal to the preset threshold vector, the described second current positioning result is determined
For the Current observation positioning result；
If second difference is greater than the preset threshold vector, the current positioning result of third is obtained, and described in calculating
Third difference between the current positioning result of third and the current predictive positioning result；
If the third difference is less than or equal to the preset threshold vector, the current positioning result of the third is determined
For the Current observation positioning result；
If the third difference is greater than the preset threshold vector, and first difference, the second difference and third is poor
The corresponding current positioning result of minimum value in value is determined as the Current observation positioning result；Wherein, described first is current fixed
Position result is first positioning result acquired based on differential GPS positioning mode, laser positioning method and vision positioning method, described
Second current positioning result is that second acquired based on differential GPS positioning mode, laser positioning method and vision positioning method is determined
Position is as a result, the current positioning result of the third is to be acquired based on differential GPS positioning mode, laser positioning method and vision positioning method
Third positioning result.
Further, the time is updated into position and posture, the current kalman gain matrix and described work as described
Preceding observation positioning result substitutes into the second preset algorithm, after calculating current predictive position and posture, the method also includes:
The Current observation matrix, the current kalman gain matrix and the time are updated covariance matrix to substitute into
In 6th preset algorithm, current predictive covariance matrix is calculated；
The current predictive position and posture, the current predictive covariance matrix and the Current observation positioning result are delayed
It deposits into local storage space.
Further, the time is updated into position and posture, the current kalman gain matrix and described work as described
Preceding observation positioning result substitutes into the second preset algorithm, after calculating current predictive position and posture, the method also includes:
The current predictive position and posture is carried out output to show.
Present invention also provides a kind of computer program products, when executing on data processing equipment, are adapted for carrying out just
The program code of beginningization there are as below methods step: current control amount, history observation bit appearance state, historical forecast position and posture are obtained
And historical forecast covariance matrix, and according to the current control amount, the history observation bit appearance state, the historical forecast position
Appearance state and the historical forecast covariance matrix, calculate time update position and posture and the time updates covariance matrix；It obtains
Current observation positioning result；Obtain Current observation matrix and Current observation covariance matrix, and by the Current observation matrix, institute
It states Current observation covariance matrix and the time updates covariance matrix and substitutes into the first preset algorithm, calculate the current card
Germania gain matrix；The time is updated into position and posture, the current kalman gain matrix and Current observation positioning
As a result it substitutes into the second preset algorithm, calculates current predictive position and posture.
It should be understood by those skilled in the art that, embodiments herein can provide as method, system or computer program
Product.Therefore, complete hardware embodiment, complete software embodiment or reality combining software and hardware aspects can be used in the application
Apply the form of example.Moreover, it wherein includes the computer of computer usable program code that the application, which can be used in one or more,
The computer program implemented in usable storage medium (including but not limited to magnetic disk storage, CD-ROM, optical memory etc.) produces
The form of product.
The application is referring to method, the process of equipment (system) and computer program product according to the embodiment of the present application
Figure and/or block diagram describe.It should be understood that every one stream in flowchart and/or the block diagram can be realized by computer program instructions
The combination of process and/or box in journey and/or box and flowchart and/or the block diagram.It can provide these computer programs
Instruct the processor of general purpose computer, special purpose computer, Embedded Processor or other programmable data processing devices to produce
A raw machine, so that being generated by the instruction that computer or the processor of other programmable data processing devices execute for real
The device for the function of being specified in present one or more flows of the flowchart and/or one or more blocks of the block diagram.
These computer program instructions, which may also be stored in, is able to guide computer or other programmable data processing devices with spy
Determine in the computer-readable memory that mode works, so that it includes referring to that instruction stored in the computer readable memory, which generates,
Enable the manufacture of device, the command device realize in one box of one or more flows of the flowchart and/or block diagram or
The function of being specified in multiple boxes.
These computer program instructions also can be loaded onto a computer or other programmable data processing device, so that counting
Series of operation steps are executed on calculation machine or other programmable devices to generate computer implemented processing, thus in computer or
The instruction executed on other programmable devices is provided for realizing in one or more flows of the flowchart and/or block diagram one
The step of function of being specified in a box or multiple boxes.
In a typical configuration, calculating equipment includes one or more processors (CPU), input/output interface, net
Network interface and memory.
Memory may include the non-volatile memory in computer-readable medium, random access memory (RAM) and/
Or the forms such as Nonvolatile memory, such as read-only memory (ROM) or flash memory (flash RAM).Memory is computer-readable Jie
The example of matter.
Computer-readable medium includes permanent and non-permanent, removable and non-removable media can be by any method
Or technology come realize information store.Information can be computer readable instructions, data structure, the module of program or other data.
The example of the storage medium of computer includes, but are not limited to phase change memory (PRAM), static random access memory (SRAM), moves
State random access memory (DRAM), other kinds of random access memory (RAM), read-only memory (ROM), electric erasable
Programmable read only memory (EEPROM), flash memory or other memory techniques, read-only disc read only memory (CD-ROM) (CD-ROM),
Digital versatile disc (DVD) or other optical storage, magnetic cassettes, tape magnetic disk storage or other magnetic storage devices
Or any other non-transmission medium, can be used for storage can be accessed by a computing device information.As defined in this article, it calculates
Machine readable medium does not include temporary computer readable media (transitory media), such as the data-signal and carrier wave of modulation.
It should also be noted that, the terms "include", "comprise" or its any other variant are intended to nonexcludability
It include so that the process, method, commodity or the equipment that include a series of elements not only include those elements, but also to wrap
Include other elements that are not explicitly listed, or further include for this process, method, commodity or equipment intrinsic want
Element.In the absence of more restrictions, the element limited by sentence "including a ...", it is not excluded that including element
There is also other identical elements in process, method, commodity or equipment.
It will be understood by those skilled in the art that embodiments herein can provide as method, system or computer program product.
Therefore, complete hardware embodiment, complete software embodiment or embodiment combining software and hardware aspects can be used in the application
Form.It is deposited moreover, the application can be used to can be used in the computer that one or more wherein includes computer usable program code
The shape for the computer program product implemented on storage media (including but not limited to magnetic disk storage, CD-ROM, optical memory etc.)
Formula.
The above is only embodiments herein, are not intended to limit this application.To those skilled in the art,
Various changes and changes are possible in this application.It is all within the spirit and principles of the present application made by any modification, equivalent replacement,
Improve etc., it should be included within the scope of the claims of this application.
Claims (10)
1. a kind of localization method of pilotless automobile characterized by comprising
Obtain current control amount, history observation bit appearance state, historical forecast position and posture and historical forecast covariance matrix, and root
According to the current control amount, the history observation bit appearance state, the historical forecast position and posture and the historical forecast association side
Poor matrix, calculates time update position and posture and the time updates covariance matrix；
Obtain Current observation positioning result；
Current observation matrix and Current observation covariance matrix are obtained, and the Current observation matrix, the Current observation are assisted
Variance matrix and the time update covariance matrix and substitute into the first preset algorithm, calculate the current kalman gain square
Battle array；
The time is updated into position and posture, the current kalman gain matrix and the Current observation positioning result substitute into the
In two preset algorithms, current predictive position and posture is calculated.
2. the method according to claim 1, wherein described observe according to the current control amount, the history
Position and posture, the historical forecast position and posture and the historical forecast covariance matrix, calculate the time update position and posture and
Time updates covariance matrix, comprising:
Local derviation processing is carried out to the history observation bit appearance state using preset function, to obtain the history observation bit appearance state
Corresponding first Jacobian matrix；
Local derviation processing is carried out to the current control amount using the preset function, to obtain the current control amount corresponding the
Two Jacobian matrixes；
By first Jacobian matrix, second Jacobian matrix, the current control amount and the historical forecast pose
State substitutes into third preset algorithm, calculates the time update position and posture；
By first Jacobian matrix, second Jacobian matrix, the historical forecast covariance matrix and customized generation
Quantity substitutes into the 4th preset algorithm, calculates the time update covariance matrix.
3. the method according to claim 1, wherein the acquisition Current observation positioning result, comprising:
The time is updated position and posture to substitute into the 5th preset algorithm, calculates current predictive positioning result；
The first current positioning result is obtained, and is calculated between the described first current positioning result and the current predictive positioning result
The first difference；
If first difference is less than or equal to preset threshold vector, the described first current positioning result is determined as described work as
Preceding observation positioning result；
If first difference is greater than the preset threshold vector, the second current positioning result is obtained, and calculate described second
The second difference between current positioning result and the current predictive positioning result；
If second difference is less than or equal to the preset threshold vector, the described second current positioning result is determined as institute
State Current observation positioning result；
If second difference is greater than the preset threshold vector, the current positioning result of third is obtained, and calculate the third
Third difference between current positioning result and the current predictive positioning result；
If the third difference is less than or equal to the preset threshold vector, the current positioning result of the third is determined as institute
State Current observation positioning result；
If the third difference is greater than the preset threshold vector, will be in first difference, the second difference and third difference
The corresponding current positioning result of minimum value be determined as the Current observation positioning result；Wherein, described first works as prelocalization knot
Fruit is first positioning result acquired based on differential GPS positioning mode, laser positioning method and vision positioning method, described second
Current positioning result is the second positioning knot acquired based on differential GPS positioning mode, laser positioning method and vision positioning method
Fruit, the current positioning result of third are the acquired based on differential GPS positioning mode, laser positioning method and vision positioning method
Three positioning results.
4. the method according to claim 1, wherein the time being updated position and posture, described working as described
Preceding kalman gain matrix and the Current observation positioning result substitute into the second preset algorithm, calculate current predictive position and posture
Later, the method also includes:
The Current observation matrix, the current kalman gain matrix and the time are updated into covariance matrix and substitute into the 6th
In preset algorithm, current predictive covariance matrix is calculated；
By the current predictive position and posture, the current predictive covariance matrix and the Current observation positioning result cache to
In local storage space.
5. method according to any of claims 1-4, which is characterized in that the time is updated pose shape described
State, the current kalman gain matrix and the Current observation positioning result substitute into the second preset algorithm, calculate current pre-
It surveys after position and posture, the method also includes:
The current predictive position and posture is carried out output to show.
6. a kind of positioning device of pilotless automobile characterized by comprising
First acquisition unit, it is pre- for obtaining current control amount, history observation bit appearance state, historical forecast position and posture and history
Survey covariance matrix；
First computing unit, for according to the current control amount, the history observation bit appearance state, the historical forecast pose
State and the historical forecast covariance matrix, calculate time update position and posture and the time updates covariance matrix；
Second acquisition unit, for obtaining Current observation positioning result；
Third acquiring unit, for obtaining Current observation matrix and Current observation covariance matrix；
Second computing unit, for updating the Current observation matrix, the Current observation covariance matrix and the time
Covariance matrix substitutes into the first preset algorithm, calculates the current kalman gain matrix；
Third computing unit, for the time to be updated position and posture, the current kalman gain matrix and described current
It observes positioning result to substitute into the second preset algorithm, calculates current predictive position and posture.
7. device according to claim 6, which is characterized in that first computing unit includes:
First local derviation module, for carrying out local derviation processing to the history observation bit appearance state using preset function, to obtain
State corresponding first Jacobian matrix of history observation bit appearance state；
Second local derviation module, for carrying out local derviation processing to the current control amount using the preset function, described in obtaining
Corresponding second Jacobian matrix of current control amount；
First computing module, for by first Jacobian matrix, second Jacobian matrix, the current control amount and
The historical forecast position and posture substitutes into third preset algorithm, calculates the time update position and posture；
Second computing module is used for first Jacobian matrix, second Jacobian matrix, the historical forecast association side
Poor matrix and customized algebraic quantity substitute into the 4th preset algorithm, calculate the time update covariance matrix.
8. device according to claim 6, which is characterized in that the second acquisition unit includes:
Third computing module substitutes into the 5th preset algorithm for the time to be updated position and posture, it is fixed to calculate current predictive
Position result；
4th computing module for obtaining the first current positioning result, and calculates the described first current positioning result and works as with described
The first difference between preceding prediction positioning result；
First determining module, for when first difference is less than or equal to preset threshold vector, described first currently to be determined
Position result is determined as the Current observation positioning result；
5th computing module, for obtaining second and working as prelocalization knot when first difference is greater than the preset threshold vector
Fruit, and calculate the second difference between the described second current positioning result and the current predictive positioning result；
Second determining module, for when second difference is less than or equal to the preset threshold vector, described second to be worked as
Prelocalization result is determined as the Current observation positioning result；
6th computing module, for obtaining third and working as prelocalization knot when second difference is greater than the preset threshold vector
Fruit, and calculate the third difference between the current positioning result of the third and the current predictive positioning result；
Third determining module, for when the third difference is less than or equal to the preset threshold vector, the third to be worked as
Prelocalization result is determined as the Current observation positioning result；
4th determining module is used for when the third difference is greater than the preset threshold vector, by first difference, second
The corresponding current positioning result of minimum value in difference and third difference is determined as the Current observation positioning result；Wherein, institute
Stating the first current positioning result is first acquired based on differential GPS positioning mode, laser positioning method and vision positioning method
Positioning result, the second current positioning result are to be obtained based on differential GPS positioning mode, laser positioning method and vision positioning method
Second positioning result arrived, the current positioning result of third are fixed based on differential GPS positioning mode, laser positioning method and vision
The third positioning result that position method acquires.
9. a kind of storage medium, which is characterized in that the storage medium includes the program of storage, wherein run in described program
When control the storage medium where equipment perform claim require 1 to the pilotless automobile described in any one of claim 5
Localization method.
10. a kind of processor, which is characterized in that the processor is for running program, wherein right of execution when described program is run
Benefit require 1 to the pilotless automobile described in any one of claim 5 localization method.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910314074.5A CN109947116B (en) | 2019-04-18 | 2019-04-18 | Positioning method and device for unmanned vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910314074.5A CN109947116B (en) | 2019-04-18 | 2019-04-18 | Positioning method and device for unmanned vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109947116A true CN109947116A (en) | 2019-06-28 |
| CN109947116B CN109947116B (en) | 2022-10-18 |
Family
ID=67015473
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910314074.5A Active CN109947116B (en) | 2019-04-18 | 2019-04-18 | Positioning method and device for unmanned vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109947116B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112256006A (en) * | 2019-07-02 | 2021-01-22 | 中国移动通信集团贵州有限公司 | Data processing method and device and electronic equipment |
| CN112987745A (en) * | 2021-03-04 | 2021-06-18 | 苏州车泊特智能科技有限公司 | Quick vehicle access system based on RTK technology |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006038650A (en) * | 2004-07-27 | 2006-02-09 | Sumitomo Precision Prod Co Ltd | Posture measuring method, posture controller, azimuth meter and computer program |
| JP2010032415A (en) * | 2008-07-30 | 2010-02-12 | Mitsubishi Electric Corp | Infrared image tracking apparatus |
| CN102706342A (en) * | 2012-05-31 | 2012-10-03 | 重庆邮电大学 | Location and environment modeling method of intelligent movable robot |
| FR3000813A1 (en) * | 2013-01-04 | 2014-07-11 | Parrot | Rotary wing drone i.e. quadricopter, has image analysis unit implementing Kalman filter estimator having representation of dynamic model of drone, with input of horizontal speed, position, linear acceleration, rotation and altitude signals |
| CN107193026A (en) * | 2017-05-06 | 2017-09-22 | 千寻位置网络有限公司 | Pseudorange positioning smooth method and system, positioning terminal |
| CN107426693A (en) * | 2017-06-01 | 2017-12-01 | 北京小度信息科技有限公司 | Localization method and device |
| CN109141427A (en) * | 2018-08-29 | 2019-01-04 | 上海理工大学 | EKF localization method under nlos environment based on distance and angle probabilistic model |
| CN109490931A (en) * | 2018-09-03 | 2019-03-19 | 天津远度科技有限公司 | Flight localization method, device and unmanned plane |
-
2019
- 2019-04-18 CN CN201910314074.5A patent/CN109947116B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006038650A (en) * | 2004-07-27 | 2006-02-09 | Sumitomo Precision Prod Co Ltd | Posture measuring method, posture controller, azimuth meter and computer program |
| JP2010032415A (en) * | 2008-07-30 | 2010-02-12 | Mitsubishi Electric Corp | Infrared image tracking apparatus |
| CN102706342A (en) * | 2012-05-31 | 2012-10-03 | 重庆邮电大学 | Location and environment modeling method of intelligent movable robot |
| FR3000813A1 (en) * | 2013-01-04 | 2014-07-11 | Parrot | Rotary wing drone i.e. quadricopter, has image analysis unit implementing Kalman filter estimator having representation of dynamic model of drone, with input of horizontal speed, position, linear acceleration, rotation and altitude signals |
| CN107193026A (en) * | 2017-05-06 | 2017-09-22 | 千寻位置网络有限公司 | Pseudorange positioning smooth method and system, positioning terminal |
| CN107426693A (en) * | 2017-06-01 | 2017-12-01 | 北京小度信息科技有限公司 | Localization method and device |
| CN109141427A (en) * | 2018-08-29 | 2019-01-04 | 上海理工大学 | EKF localization method under nlos environment based on distance and angle probabilistic model |
| CN109490931A (en) * | 2018-09-03 | 2019-03-19 | 天津远度科技有限公司 | Flight localization method, device and unmanned plane |
Non-Patent Citations (6)
| Title |
|---|
| MENGJIAO ZHAO: "Adaptive_wavelet_packet_thresholding_with_iterative_Kalman_filter_for_speech_enhancement", 《2017 IEEE GLOBAL CONFERENCE ON SIGNAL AND INFORMATION PROCESSING》 * |
| 刘丽丽等: "一种SINS/CNS/GNSS组合导航滤波算法", 《大地测量与地球动力学》 * |
| 刘志成: "《在线传感器性能评估与故障诊断方法研究》", 31 July 2013 * |
| 周建斌: "《核信号数字分析与处理》", 28 February 2017 * |
| 张毅: "《移动机器人技术基础与制作》", 31 January 2013 * |
| 黄椰: "基于双目立体视觉的船舶轨迹跟踪算法研究", 《计算机科学》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112256006A (en) * | 2019-07-02 | 2021-01-22 | 中国移动通信集团贵州有限公司 | Data processing method and device and electronic equipment |
| CN112256006B (en) * | 2019-07-02 | 2023-04-28 | 中国移动通信集团贵州有限公司 | Data processing method and device and electronic equipment |
| CN112987745A (en) * | 2021-03-04 | 2021-06-18 | 苏州车泊特智能科技有限公司 | Quick vehicle access system based on RTK technology |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109947116B (en) | 2022-10-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Murali et al. | Perception-aware trajectory generation for aggressive quadrotor flight using differential flatness | |
| CN109655066A (en) | One kind being based on the unmanned plane paths planning method of Q (λ) algorithm | |
| CN101871782B (en) | Position error forecasting method for GPS (Global Position System)/MEMS-INS (Micro-Electricomechanical Systems-Inertial Navigation System) integrated navigation system based on SET2FNN | |
| Edelkamp et al. | Monte-Carlo tree search for logistics | |
| Hasberg et al. | Simultaneous localization and mapping for path-constrained motion | |
| CN111079936B (en) | Wave fin propulsion underwater operation robot tracking control method based on reinforcement learning | |
| CN110488818A (en) | A kind of robot localization method, apparatus and robot based on laser radar | |
| CN109947116A (en) | The localization method and device of pilotless automobile | |
| Stevšić et al. | Sample efficient learning of path following and obstacle avoidance behavior for quadrotors | |
| Keivan et al. | Realtime simulation-in-the-loop control for agile ground vehicles | |
| Kapania | Trajectory planning and control for an autonomous race vehicle | |
| WO2022091305A1 (en) | Behavior estimation device, behavior estimation method, route generation device, route generation method, and computer-readable recording medium | |
| Diwan et al. | Fast nonlinear model predictive controller using parallel PSO based on divide and conquer approach | |
| CN116859979A (en) | Unmanned aerial vehicle motion path planning method, unmanned aerial vehicle and storage medium | |
| Hillebrand et al. | A design methodology for deep reinforcement learning in autonomous systems | |
| Sahraeekhanghah et al. | PA-FaSTrack: Planner-Aware Real-Time Guaranteed Safe Planning | |
| Meyer | On course towards model-free guidance: A self-learning approach to dynamic collision avoidance for autonomous surface vehicles | |
| CN115826594A (en) | Unmanned underwater vehicle switching topology formation control method independent of dynamic model parameters | |
| Feng et al. | Mobile robot obstacle avoidance based on deep reinforcement learning | |
| Wadi et al. | A novel localization-free approach to system identification for underwater vehicles using a Universal Adaptive Stabilizer | |
| Afzali et al. | A Modified Convergence DDPG Algorithm for Robotic Manipulation | |
| NO20221206A1 (en) | Ship maneuver calculation device | |
| Alhadhrami et al. | Drones tracking adaptation using reinforcement learning: Proximal policy optimization | |
| Ashokaraj et al. | Feature based robot navigation: using fuzzy logic and interval analysis | |
| Xu | Bioinspired Intelligent Control of Autonomous Robots with State Estimation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |