CN109947116B - Positioning method and device for unmanned vehicle - Google Patents

Positioning method and device for unmanned vehicle Download PDF

Info

Publication number
CN109947116B
CN109947116B CN201910314074.5A CN201910314074A CN109947116B CN 109947116 B CN109947116 B CN 109947116B CN 201910314074 A CN201910314074 A CN 201910314074A CN 109947116 B CN109947116 B CN 109947116B
Authority
CN
China
Prior art keywords
current
positioning result
pose state
observation
matrix
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
CN201910314074.5A
Other languages
Chinese (zh)
Other versions
CN109947116A (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.)
Beijing Zhuxian Technology Co Ltd
Original Assignee
Beijing Zhuxian Technology Co Ltd
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 Beijing Zhuxian Technology Co Ltd filed Critical Beijing Zhuxian Technology Co Ltd
Priority to CN201910314074.5A priority Critical patent/CN109947116B/en
Publication of CN109947116A publication Critical patent/CN109947116A/en
Application granted granted Critical
Publication of CN109947116B publication Critical patent/CN109947116B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a method and a device for positioning an unmanned automobile, relates to the technical field of unmanned driving, and accurately positions the pose of the unmanned automobile. The invention comprises the following steps: acquiring a current control quantity, a historical observation pose state, a historical prediction pose state and a historical prediction covariance matrix, and calculating a time update pose state and a time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix; acquiring a current observation positioning result; acquiring a current observation matrix and a current observation covariance matrix, substituting the current observation matrix, the current observation covariance matrix and a time updating covariance matrix into a first preset algorithm, and calculating a current Kalman gain matrix; and substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate the current prediction pose state. The invention is suitable for positioning the pose of the unmanned automobile.

Description

Positioning method and device for unmanned vehicle
Technical Field
The invention relates to the technical field of unmanned driving, in particular to a positioning method and a positioning device for an unmanned automobile.
Background
With the continuous development and the increasing popularization of unmanned technology, more and more closed or semi-closed scenes begin to adopt the unmanned technology to assist and provide support for the operation process, and the port environment is one of the scenes. In the process of operation of the unmanned vehicle, the unmanned vehicle needs to acquire the self positioning result in real time, so that the safety and the working stability of the unmanned vehicle are ensured, and therefore, how to accurately position the pose of the unmanned vehicle is crucial to acquire the positioning result of the unmanned vehicle.
At present, a positioning result of an unmanned vehicle is usually obtained by any one of three positioning methods, namely a differential GPS positioning method, a laser positioning method and a visual positioning method, however, because a port environment is complex and positions of a rail crane and a container in the port environment change frequently, the existing positioning method cannot accurately position the pose of the unmanned vehicle, and the accuracy of the positioning result obtained by the unmanned vehicle through the existing positioning method is low.
Disclosure of Invention
In view of this, the present invention provides a method and an apparatus for positioning an unmanned vehicle, and aims to accurately position the pose of the unmanned vehicle, so as to obtain a positioning result with higher accuracy.
In order to achieve the above purpose, the present invention mainly provides the following technical solutions:
in a first aspect, the present invention provides a method for locating an unmanned vehicle, the method comprising:
acquiring a current control quantity, a historical observation pose state, a historical prediction pose state and a historical prediction covariance matrix, and calculating a time update pose state and a time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix;
acquiring a current observation positioning result;
acquiring a current observation matrix and a current observation covariance matrix, substituting the current observation matrix, the current observation covariance matrix and the time updating covariance matrix into a first preset algorithm, and calculating the current Kalman gain matrix;
and substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate the current prediction pose state.
Optionally, the calculating a time update pose state and a time update covariance matrix according to the current controlled variable, the historical observation pose state, the historical prediction pose state, and the historical prediction covariance matrix includes:
performing partial derivation processing on the historical observation pose state by using a preset function to obtain a first Jacobian matrix corresponding to the historical observation pose state;
performing partial derivative processing on the current control quantity by using the preset function to obtain a second Jacobian matrix corresponding to the current control quantity;
substituting the first Jacobian matrix, the second Jacobian matrix, the current control quantity and the historical predicted pose state into a third preset algorithm, and calculating the time update pose state;
and substituting the first Jacobian matrix, the second Jacobian matrix, the historical prediction covariance matrix and the custom generation number into a fourth preset algorithm, and calculating the time updating covariance matrix.
Optionally, the obtaining of the current observation positioning result includes:
substituting the time updating pose state into a fifth preset algorithm, and calculating a current prediction positioning result;
acquiring a first current positioning result, and calculating a first difference value between the first current positioning result and the current predicted positioning result;
if the first difference is smaller than or equal to a preset threshold vector, determining the first current positioning result as the current observation positioning result;
if the first difference is larger than the preset threshold vector, obtaining a second current positioning result, and calculating a second difference between the second current positioning result and the current predicted positioning result;
if the second difference is smaller than or equal to the preset threshold vector, determining the second current positioning result as the current observation positioning result;
if the second difference is larger than the preset threshold vector, obtaining a third current positioning result, and calculating a third difference between the third current positioning result and the current predicted positioning result;
if the third difference is smaller than or equal to the preset threshold vector, determining the third current positioning result as the current observation positioning result;
if the third difference is greater than the preset threshold vector, determining a current positioning result corresponding to the minimum value of the first difference, the second difference and the third difference as the current observation positioning result; the first current positioning result is a first positioning result obtained based on a differential GPS positioning method, a laser positioning method and a visual positioning method, the second current positioning result is a second positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method, and the third current positioning result is a third positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method.
Optionally, after the time update pose state, the current kalman gain matrix, and the current observation positioning result are substituted into a second preset algorithm to calculate a current predicted pose state, the method further includes:
substituting the current observation matrix, the current Kalman gain matrix and the time updating covariance matrix into a sixth preset algorithm to calculate a current prediction covariance matrix;
and caching the current prediction pose state, the current prediction covariance matrix and the current observation positioning result into a local storage space.
Optionally, after the time update pose state, the current kalman gain matrix, and the current observation positioning result are substituted into a second preset algorithm to calculate a current predicted pose state, the method further includes:
and outputting and displaying the current predicted pose state.
In a second aspect, the present invention also provides a positioning device for an unmanned vehicle, the device comprising:
the first acquisition unit is used for acquiring the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix;
a first calculation unit, configured to calculate a time update pose state and a time update covariance matrix according to the current control amount, the historical observation pose state, the historical prediction pose state, and the historical prediction covariance matrix;
the second acquisition unit is used for acquiring a current observation positioning result;
the third acquisition unit is used for acquiring a current observation matrix and a current observation covariance matrix;
the second calculation unit is used for substituting the current observation matrix, the current observation covariance matrix and the time updating covariance matrix into a first preset algorithm to calculate the current Kalman gain matrix;
and the third calculation unit is used for substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate the current prediction pose state.
Optionally, the first computing unit includes:
the first partial derivative module is used for performing partial derivative processing on the historical observation pose state by using a preset function so as to obtain a first Jacobian matrix corresponding to the historical observation pose state;
the second partial derivative module is used for performing partial derivative processing on the current control quantity by using the preset function so as to obtain a second Jacobian matrix corresponding to the current control quantity;
the first calculation module is used for substituting the first Jacobian matrix, the second Jacobian matrix, the current control quantity and the historical predicted pose state into a third preset algorithm to calculate the time updating pose state;
and the second calculation module is used for substituting the first Jacobian matrix, the second Jacobian matrix, the historical prediction covariance matrix and the custom generation number into a fourth preset algorithm to calculate the time updating covariance matrix.
Optionally, the second obtaining unit includes:
the third calculation module is used for substituting the time updating pose state into a fifth preset algorithm to calculate a current prediction positioning result;
the fourth calculation module is used for acquiring a first current positioning result and calculating a first difference value between the first current positioning result and the current predicted positioning result;
a first determining module, configured to determine the first current positioning result as the current observation positioning result when the first difference is smaller than or equal to a preset threshold vector;
a fifth calculating module, configured to obtain a second current positioning result when the first difference is greater than the preset threshold vector, and calculate a second difference between the second current positioning result and the current predicted positioning result;
a second determining module, configured to determine the second current positioning result as the current observed positioning result when the second difference is smaller than or equal to the preset threshold vector;
a sixth calculating module, configured to obtain a third current positioning result when the second difference is greater than the preset threshold vector, and calculate a third difference between the third current positioning result and the current predicted positioning result;
a third determining module, configured to determine the third current positioning result as the current observation positioning result when the third difference is smaller than or equal to the preset threshold vector;
a fourth determining module, configured to determine, when the third difference is greater than the preset threshold vector, a current positioning result corresponding to a minimum value of the first difference, the second difference, and the third difference as the current observation positioning result; the first current positioning result is a first positioning result obtained based on a differential GPS positioning method, a laser positioning method and a visual positioning method, the second current positioning result is a second positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method, and the third current positioning result is a third positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method.
Optionally, the apparatus further comprises:
a fourth calculating unit, configured to substitute the time update pose state, the current kalman gain matrix, and the current observation positioning result into a second preset algorithm, calculate a current prediction pose state, substitute the current observation matrix, the current kalman gain matrix, and the time update covariance matrix into a sixth preset algorithm, and calculate a current prediction covariance matrix;
and the cache unit is used for caching the current prediction pose state, the current prediction covariance matrix and the current observation positioning result into a local storage space.
Optionally, the apparatus further comprises:
and the output unit is used for substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm in the third calculation unit, calculating the current prediction pose state, and then outputting and displaying the current prediction pose state.
In order to achieve the above object, according to a third aspect of the present invention, there is provided a storage medium comprising a stored program, wherein when the program runs, an apparatus on which the storage medium is located is controlled to execute the above method for locating an unmanned vehicle.
In order to achieve the above object, according to a fourth aspect of the present invention, a processor for running a program is provided, wherein the program is run to execute the above-mentioned positioning method for an unmanned vehicle.
By means of the technical scheme, the technical scheme provided by the invention at least has the following advantages:
compared with the prior art that the unmanned vehicle acquires the self-corresponding positioning result based on any one of three positioning methods, namely a differential GPS positioning method, a laser positioning method and a visual positioning method, the positioning method and the positioning device of the unmanned vehicle can enable the unmanned vehicle to acquire the self-corresponding current control quantity, a historical observation pose state, a historical prediction pose state and a historical prediction covariance matrix, calculate the self-corresponding time update pose state and time update covariance matrix according to the acquired current control quantity, historical observation pose state, historical prediction pose state and historical prediction covariance matrix, acquire the self-corresponding current observation matrix and current observation covariance matrix after acquiring the self-corresponding current observation positioning result, calculate the self-corresponding current Kalman gain matrix according to the acquired current observation matrix, current observation covariance matrix and time update covariance matrix, and calculate the self-corresponding current Kalman gain matrix according to the calculated current Kalman gain matrix, the time update pose state and the current observation positioning result. Because the unmanned vehicle calculates the current predicted pose state corresponding to the unmanned vehicle based on the current observation positioning result, the current Kalman gain matrix and the time update pose state corresponding to the unmanned vehicle, namely, the current predicted pose state corresponding to the unmanned vehicle at the current moment is predicted based on an EKF (Extended Kalman Filter) frame, the unmanned vehicle can accurately position the pose of the unmanned vehicle in a complex port environment, and the safety and the working stability of the unmanned vehicle in the working process can be ensured.
The above description is only an overview of the technical solutions of the present invention, and the present invention can be implemented in accordance with the content of the description so as to make the technical means of the present invention more clearly understood, and the above and other objects, features, and advantages of the present invention will be more clearly understood.
Drawings
Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The drawings are only for purposes of illustrating the preferred embodiments and are not to be construed as limiting the invention. Also, like reference numerals are used to refer to like parts throughout the drawings. In the drawings:
FIG. 1 is a flow chart illustrating a method for locating an unmanned vehicle according to an embodiment of the present invention;
FIG. 2 is a flow chart illustrating another method for locating an unmanned vehicle according to an embodiment of the present invention;
FIG. 3 is a block diagram illustrating a positioning apparatus for an unmanned vehicle according to an embodiment of the present invention;
fig. 4 is a block diagram illustrating a positioning apparatus of another unmanned vehicle according to an embodiment of the present invention.
Detailed Description
Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.
The embodiment of the invention provides a positioning method of an unmanned automobile, which comprises the following steps of:
101. and obtaining the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix, and calculating the time update pose state and the time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix.
The current control quantity corresponding to the unmanned vehicle is a change value of angular velocity and acceleration of the unmanned vehicle from the last moment to the current moment, and specifically, the unmanned vehicle can obtain the current control quantity corresponding to the unmanned vehicle through an Inertial Measurement Unit (IMU); the observation positioning result corresponding to the unmanned automobile is a positioning result obtained based on the existing positioning method, and comprises the orientation and the position corresponding to the unmanned automobile, and the observation pose state corresponding to the unmanned automobile is a positioning result obtained by converting the observation positioning result and comprises the orientation, the position and the speed corresponding to the unmanned automobile, so that the historical observation pose state corresponding to the unmanned automobile is the observation pose state corresponding to the unmanned automobile at the last moment and comprises the orientation, the position and the speed corresponding to the unmanned automobile at the last moment; the predicted pose state corresponding to the unmanned automobile is a positioning result obtained by calculation in the process of predicting the pose state of the unmanned automobile, and the positioning result comprises the orientation, the position and the speed corresponding to the unmanned automobile, so that the historical predicted pose state corresponding to the unmanned automobile is the predicted pose state corresponding to the unmanned automobile at the last moment; the predicted covariance matrix corresponding to the unmanned automobile is a covariance matrix obtained through calculation in the process of operation of predicting the pose state of the unmanned automobile, and therefore the historical predicted covariance matrix corresponding to the unmanned automobile is the predicted covariance matrix corresponding to the unmanned automobile at the last moment.
In the embodiment of the invention, after the unmanned vehicle obtains an observation positioning result based on the existing positioning method and initializes the pose state of the unmanned vehicle based on the observation positioning result, the predicted pose state (namely the current predicted pose state) corresponding to the unmanned vehicle at the current time can be calculated based on the observation positioning result (namely the current observation positioning result) corresponding to the current time. When the unmanned vehicle calculates the current predicted pose state corresponding to the unmanned vehicle based on the current observation positioning result corresponding to the unmanned vehicle, the unmanned vehicle firstly needs to acquire the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix corresponding to the unmanned vehicle, and carries out time updating processing on the acquired historical prediction pose state so as to acquire the time updating pose state corresponding to the unmanned vehicle, and carries out time updating processing on the acquired historical prediction covariance matrix so as to acquire the time updating covariance matrix corresponding to the unmanned vehicle, namely calculates the time updating state and the time updating covariance matrix corresponding to the unmanned vehicle according to the acquired current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix, so that the current predicted pose state corresponding to the unmanned vehicle is calculated based on the current observation positioning result, the time updating pose state and the time updating covariance matrix corresponding to the unmanned vehicle.
102. And acquiring a current observation positioning result.
In the embodiment of the invention, after the unmanned vehicle calculates the corresponding time update pose state and the time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix corresponding to the unmanned vehicle, the corresponding current observation positioning result can be obtained, so that the corresponding current prediction pose state can be calculated based on the corresponding current observation positioning result. Specifically, in the embodiment of the present invention, after obtaining the positioning result corresponding to the current time based on any one of the existing positioning methods, the unmanned vehicle may determine the positioning result as the current predicted pose state corresponding to the unmanned vehicle itself, or after obtaining the positioning result corresponding to the current time based on each of the existing positioning methods, determine the optimal positioning result as the current predicted pose state corresponding to the unmanned vehicle itself, but the present invention is not limited thereto.
103. And acquiring a current observation matrix and a current observation covariance matrix, substituting the current observation matrix, the current observation covariance matrix and a time updating covariance matrix into a first preset algorithm, and calculating a current Kalman gain matrix.
In the embodiment of the present invention, after obtaining the current observation positioning result corresponding to the unmanned vehicle, the unmanned vehicle may obtain the observation matrix and the observation covariance matrix (i.e. the current observation matrix and the current observation covariance matrix) corresponding to the unmanned vehicle at the current time, and substitute the obtained current observation matrix, the current observation covariance matrix, and the time update covariance matrix calculated in step 101 into a first preset algorithm, so as to calculate the kalman gain matrix (i.e. the current kalman gain matrix) corresponding to the unmanned vehicle at the current time, where the first preset algorithm is specifically as follows:
K k ＝P k|k-1 H k T (H k P k|k-1 H k T +R k ) -1
wherein, K k For the current Kalman gain matrix, P k|k-1 Updating the covariance matrix for time, H k For the current observation matrix, R k Is the current observed covariance matrix.
104. And substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate the current prediction pose state.
In the embodiment of the present invention, after the unmanned vehicle obtains the current Kalman gain matrix corresponding to itself through the calculation in step 103, the time update pose state obtained through the calculation in step 101, the current Kalman gain matrix obtained through the calculation in step 103, and the current observation positioning result obtained in step 102 may be substituted into a second preset algorithm, so as to calculate the predicted pose state (i.e., the current predicted pose state) corresponding to itself at the current time, and predict the positioning result corresponding to itself at the current time based on an EKF (Extended Kalman Filter) frame, where the second preset algorithm specifically is as follows:
X k|k ＝X k|k-1 +K k [Z k -h(X k|k-1 )]
wherein X k|k For the current predicted pose state, X k|k-1 Updating pose states for time, K k For the current Kalman gain matrix, Z k H is an identity matrix for the current observation positioning result.
Compared with the prior art that the unmanned vehicle acquires the self-corresponding positioning result based on any one of three positioning methods, namely a differential GPS positioning method, a laser positioning method and a visual positioning method, the positioning method provided by the embodiment of the invention can enable the unmanned vehicle to firstly acquire the self-corresponding current control quantity, historical observation pose state, historical prediction pose state and historical prediction covariance matrix, then calculate the self-corresponding time update pose state and time update covariance matrix according to the acquired current control quantity, historical observation pose state, historical prediction pose state and historical prediction covariance matrix, acquire the self-corresponding current observation matrix and current observation covariance matrix after acquiring the self-corresponding current observation positioning result, calculate the self-corresponding current Kalman gain matrix according to the acquired current observation matrix, current observation covariance matrix and time update covariance matrix, and calculate the self-corresponding current Kalman gain matrix according to the calculated current observation pose state and current prediction positioning result. Because the unmanned vehicle calculates the current predicted pose state corresponding to the unmanned vehicle based on the current observation positioning result, the current Kalman gain matrix and the time update pose state corresponding to the unmanned vehicle, namely, the current predicted pose state corresponding to the unmanned vehicle at the current moment is predicted based on an EKF (Extended Kalman Filter) frame, the unmanned vehicle can accurately position the pose of the unmanned vehicle in a complex port environment, and the safety and the working stability of the unmanned vehicle in the working process can be ensured.
To be described in more detail below, an embodiment of the present invention provides another positioning method for an unmanned vehicle, and in particular, a specific method for calculating a time update pose state and a time update covariance matrix according to a current control quantity, a historical observation pose state, a historical prediction pose state, and a historical prediction covariance matrix of the unmanned vehicle, and a specific method for the unmanned vehicle to obtain a current observation positioning result, as shown in fig. 2, specifically, the method includes:
201. and acquiring the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix.
In step 201, the current control quantity, the historical observation pose state, the historical prediction pose state, and the historical prediction covariance matrix may be obtained by referring to the description of the corresponding part in fig. 1, and the details of the embodiment of the present invention will not be described here.
202. And calculating a time updating pose state and a time updating covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix.
In the embodiment of the invention, after acquiring the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix corresponding to the unmanned vehicle, the unmanned vehicle can perform time updating processing on the acquired historical prediction pose state so as to acquire the time updating pose state corresponding to the unmanned vehicle, and perform time updating processing on the acquired historical prediction covariance matrix so as to acquire the time updating covariance matrix corresponding to the unmanned vehicle (namely, the time updating pose state and the time updating covariance matrix corresponding to the unmanned vehicle are calculated according to the acquired current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix). How the unmanned vehicle calculates the time update pose state and the time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state, and the historical prediction covariance matrix will be described in detail below.
(1) And carrying out partial derivation processing on the current control quantity by using the preset function so as to obtain a second Jacobian matrix corresponding to the current control quantity.
In the embodiment of the invention, after the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix corresponding to the unmanned vehicle are obtained, the historical observation pose state and the current control quantity which are respectively obtained by using the preset function are subjected to partial derivation processing, so that a first Jacobian matrix corresponding to the historical observation pose state and a second Jacobian matrix corresponding to the current control quantity are obtained.
(2) And substituting the first Jacobian matrix, the second Jacobian matrix, the current control quantity and the historical predicted pose state into a third preset algorithm, and calculating time updating pose state.
In the embodiment of the present invention, after obtaining a first jacobian matrix corresponding to a historical observation pose state and a second jacobian matrix corresponding to a current control amount, the unmanned vehicle may perform time update processing on a historical prediction pose state based on the first jacobian matrix and the second jacobian matrix, so as to obtain a time update pose state corresponding to the unmanned vehicle, that is, the first jacobian matrix, the second jacobian matrix, the current control amount, and the historical prediction pose state are substituted into a third preset algorithm, so as to calculate the time update pose state, where the third preset algorithm is specifically as follows:
X k|k-1 ＝AX k-1|k-1 +Bu k
wherein, X k|k-1 Updating pose states for time, A is a first Jacobian matrix corresponding to historical observation pose states, X k-1|k-1 Predicting the pose state for history, B is a second Jacobian matrix corresponding to the current control quantity, u k Is the current control amount.
(3) And substituting the first Jacobian matrix, the second Jacobian matrix, the historical prediction covariance matrix and the custom generation number into a fourth preset algorithm, and calculating a time update covariance matrix.
The self-defined generation number is a self-defined generation number corresponding to the unmanned automobile at the last moment.
In the embodiment of the invention, after the unmanned vehicle calculates the corresponding time update pose state according to the first Jacobian matrix, the second Jacobian matrix, the current control quantity and the historical prediction pose state, the time update processing can be carried out on the historical prediction covariance matrix based on the first Jacobian matrix and the second Jacobian matrix, so as to obtain the corresponding time update covariance matrix, namely, the first Jacobian matrix, the second Jacobian matrix, the historical prediction covariance matrix and the custom number are substituted into a fourth preset algorithm, so as to calculate the time update covariance matrix, wherein the fourth preset algorithm is as follows:
P k|k-1 ＝A P k-1|k-1 A T +BQ k-1 B T
wherein, P k|k-1 Updating covariance for timeA is a first Jacobian matrix corresponding to the historical observation pose state, P k-1|k-1 The covariance matrix is historical prediction, B is a second Jacobian matrix corresponding to the current control quantity, Q k-1 Is a custom generation number.
203. And obtaining a current observation positioning result.
In the embodiment of the invention, after the unmanned vehicle calculates the corresponding time updating pose state and the time updating covariance matrix according to the corresponding current control quantity, the corresponding historical observation pose state, the corresponding historical prediction pose state and the corresponding historical prediction covariance matrix, the corresponding current observation positioning result can be obtained, so that the corresponding current prediction pose state can be calculated based on the corresponding current observation positioning result. How the unmanned vehicle acquires the current observation positioning result will be described in detail below.
(1) The unmanned automobile substitutes the time updating pose state into a fifth preset algorithm so as to calculate the current prediction positioning result, wherein the fifth preset algorithm is as follows:
Z k|k-1 ＝h(X k|k-1 )
wherein, Z k|k-1 For the current predicted positioning result, X k|k-1 And updating the pose state for time, wherein h is an identity matrix.
(2) The unmanned vehicle obtains a first current positioning result, and calculates a first difference between the first current positioning result and a current predicted positioning result, that is, the first difference = the first current positioning result — the current predicted positioning result.
(3) And when the unmanned automobile judges that the first difference is smaller than or equal to the preset threshold vector, the unmanned automobile determines the first current positioning result as the current observation positioning result.
(4) And when the unmanned automobile judges that the first difference is larger than the preset threshold vector, the unmanned automobile acquires a second current positioning result, and calculates a second difference between the second current positioning result and the current predicted positioning result, namely the second difference = the second current positioning result-the current predicted positioning result.
(5) And when the unmanned automobile judges that the second difference value is smaller than or equal to the preset threshold vector, the unmanned automobile determines the second current positioning result as the current observation positioning result.
(6) And when the unmanned automobile judges that the second difference is larger than the preset threshold vector, the unmanned automobile acquires a third current positioning result, and calculates a third difference between the third current positioning result and the current predicted positioning result, namely the third difference = the third current positioning result-the current predicted positioning result.
(7) And when the unmanned automobile judges that the third difference is smaller than or equal to the preset threshold vector, the unmanned automobile determines the third current positioning result as the current observation positioning result.
(8) And when the unmanned vehicle determines that the third difference is larger than the preset threshold vector, the unmanned vehicle determines the current positioning result corresponding to the minimum value of the first difference, the second difference and the third difference as the current observation positioning result.
It should be noted that the first current positioning result is a first positioning result obtained by the unmanned vehicle at the current time based on the differential GPS positioning method, the laser positioning method, and the visual positioning method, the second current positioning result is a second positioning result obtained by the unmanned vehicle at the current time based on the differential GPS positioning method, the laser positioning method, and the visual positioning method, and the third current positioning result is a third positioning result obtained by the unmanned vehicle at the current time based on the differential GPS positioning method, the laser positioning method, and the visual positioning method, for example, the first positioning result obtained by the unmanned vehicle at the current time based on the differential GPS positioning method, the second positioning result obtained based on the laser positioning method, and the third positioning result obtained based on the visual positioning method are the first current positioning result, the second current positioning result obtained based on the laser positioning method, and the third current positioning result obtained based on the visual positioning method.
204. And acquiring a current observation matrix and a current observation covariance matrix, substituting the current observation matrix, the current observation covariance matrix and a time updating covariance matrix into a first preset algorithm, and calculating a current Kalman gain matrix.
In step 204, the current observation matrix and the current observation covariance matrix are obtained, and the current observation matrix, the current observation covariance matrix and the time update covariance matrix are substituted into the first preset algorithm to calculate the current kalman gain matrix, which may refer to the description of the corresponding part in fig. 1, and will not be described herein again in the embodiments of the present invention.
205. And substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate the current prediction pose state.
In step 205, the time update pose state, the current kalman gain matrix, and the current observation positioning result are substituted into a second preset algorithm to calculate the current predicted pose state, which may refer to the description of the corresponding part in fig. 1, and will not be described herein again in the embodiments of the present invention.
Further, in the embodiment of the present invention, after the unmanned vehicle calculates the current predicted pose state corresponding to the unmanned vehicle according to the time update pose state, the current kalman gain matrix, and the current observation positioning result corresponding to the unmanned vehicle, the current observation matrix obtained in step 204, the current kalman gain matrix obtained in step 204, and the time update covariance matrix obtained in step 202 may be substituted into a sixth preset algorithm, so as to calculate the current predicted covariance matrix corresponding to the unmanned vehicle, and the current predicted covariance matrix obtained in step 205, the current predicted pose state obtained in step 205, and the current observation positioning result obtained in step 203 may be cached in a local storage space, so that the current predicted covariance matrix cached in the current operation may be used as the historical predicted covariance matrix, the current predicted pose state may be used as the historical predicted covariance state, and the current observation positioning result may be used as the current observation positioning result to calculate the sixth predicted pose state corresponding to the next time, where the sixth preset algorithm is as follows:
P k|k ＝(I-K k H k )P k|k-1
wherein, P k|k For the current prediction covariance matrix, I is a matrix with all 1 elements, K k For the current Kalman gain matrix, H k For the current observation matrix, P k|k-1 The covariance matrix is updated for time.
206. And outputting and displaying the current predicted pose state.
In the embodiment of the invention, after the unmanned automobile updates the pose state, the current Kalman gain matrix and the current observation positioning result according to the time corresponding to the unmanned automobile and calculates and obtains the current prediction pose state corresponding to the unmanned automobile, the calculated current prediction pose state can be output and displayed, so that a worker can know the corresponding direction, position and speed of the unmanned automobile at the current moment.
In order to achieve the above object, according to another aspect of the present invention, an embodiment of the present invention further provides a storage medium, where the storage medium includes a stored program, where the program is executed to control a device on which the storage medium is located to perform the above-mentioned method for locating an unmanned vehicle.
In order to achieve the above object, according to another aspect of the present invention, there is further provided a processor for executing a program, where the program executes the locating method of the unmanned vehicle.
Further, as an implementation of the method shown in fig. 1 and fig. 2, another embodiment of the present invention further provides a positioning device for an unmanned vehicle. The embodiment of the apparatus corresponds to the embodiment of the method, and for convenience of reading, details in the embodiment of the apparatus are not repeated one by one, but it should be clear that the apparatus in the embodiment can correspondingly implement all the contents in the embodiment of the method. The device is applied to accurate position the position appearance to unmanned vehicle to obtain and obtain the higher location result of accuracy, specifically as shown in fig. 3, the device includes:
a first obtaining unit 31, configured to obtain a current control amount, a historical observation pose state, a historical prediction pose state, and a historical prediction covariance matrix;
a first calculation unit 32 configured to calculate a time update pose state and a time update covariance matrix based on the current control amount, the historical observation pose state, the historical predicted pose state, and the historical predicted covariance matrix;
a second obtaining unit 33, configured to obtain a current observation positioning result;
a third obtaining unit 34, configured to obtain a current observation matrix and a current observation covariance matrix;
a second calculating unit 35, configured to substitute the current observation matrix, the current observation covariance matrix, and the time update covariance matrix into a first preset algorithm, and calculate the current kalman gain matrix;
and a third calculating unit 36, configured to substitute the time update pose state, the current kalman gain matrix, and the current observation positioning result into a second preset algorithm, so as to calculate a current predicted pose state.
Further, as shown in fig. 4, the first calculation unit 32 includes:
a first derivation module 321, configured to perform derivation processing on the historical observation pose state by using a preset function to obtain a first jacobian matrix corresponding to the historical observation pose state;
a second partial derivative module 322, configured to perform partial derivative processing on the current controlled variable by using the preset function to obtain a second jacobian matrix corresponding to the current controlled variable;
a first calculating module 323, configured to substitute the first jacobian matrix, the second jacobian matrix, the current control amount, and the historical predicted pose state into a third preset algorithm, and calculate the time update pose state;
a second calculating module 324, configured to substitute the first jacobian matrix, the second jacobian matrix, the historical prediction covariance matrix, and the number of custom generations into a fourth preset algorithm to calculate the time update covariance matrix.
Further, as shown in fig. 4, the second acquiring unit 33 includes:
the third calculating module 331 is configured to substitute the time update pose state into a fifth preset algorithm to calculate a current predicted positioning result;
a fourth calculating module 332, configured to obtain a first current positioning result, and calculate a first difference between the first current positioning result and the current predicted positioning result;
a first determining module 333, configured to determine the first current positioning result as the current observed positioning result when the first difference is smaller than or equal to a preset threshold vector;
a fifth calculating module 334, configured to, when the first difference is greater than the preset threshold vector, obtain a second current positioning result, and calculate a second difference between the second current positioning result and the current predicted positioning result;
a second determining module 335, configured to determine the second current positioning result as the current observed positioning result when the second difference is smaller than or equal to the preset threshold vector;
a sixth calculating module 336, configured to obtain a third current positioning result when the second difference is greater than the preset threshold vector, and calculate a third difference between the third current positioning result and the current predicted positioning result;
a third determining module 337, configured to determine the third current positioning result as the current observed positioning result when the third difference is less than or equal to the preset threshold vector;
a fourth determining module 338, configured to determine, when the third difference is greater than the preset threshold vector, a current positioning result corresponding to a minimum value of the first difference, the second difference, and the third difference as the current observed positioning result; the first current positioning result is a first positioning result obtained based on a differential GPS positioning method, a laser positioning method and a visual positioning method, the second current positioning result is a second positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method, and the third current positioning result is a third positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method.
Further, as shown in fig. 4, the apparatus further includes:
a fourth calculating unit 37, configured to substitute the time update pose state, the current kalman gain matrix, and the current observation positioning result into a second preset algorithm, and substitute the current observation matrix, the current kalman gain matrix, and the time update covariance matrix into a sixth preset algorithm after the third calculating unit 36 calculates the current prediction pose state, and calculate the current prediction covariance matrix;
a caching unit 38, configured to cache the current predicted pose state, the current predicted covariance matrix, and the current observation positioning result in a local storage space.
Further, as shown in fig. 4, the apparatus further includes:
and an output unit 39, configured to substitute the time update pose state, the current kalman gain matrix, and the current observation positioning result into a second preset algorithm in the third calculation unit 36, calculate a current predicted pose state, and output and display the current predicted pose state.
Compared with the prior art that the unmanned vehicle acquires the self-corresponding positioning result based on any one of three positioning methods, namely a differential GPS positioning method, a laser positioning method and a visual positioning method, the positioning method and the positioning device of the unmanned vehicle provided by the embodiment of the invention can enable the unmanned vehicle to firstly acquire the self-corresponding current control quantity, historical observation pose state, historical prediction pose state and historical prediction covariance matrix, then calculate the self-corresponding time update pose state and time update covariance matrix according to the acquired current control quantity, historical observation pose state, historical prediction pose state and historical prediction covariance matrix, acquire the self-corresponding current observation matrix and current observation covariance matrix after acquiring the self-corresponding current observation positioning result, calculate the self-corresponding current Kalman gain matrix according to the acquired current observation matrix, current observation covariance matrix and time update covariance matrix, and calculate the self-corresponding current Kalman gain matrix according to the calculated current gain matrix, the time update pose state and current observation positioning result. Because the unmanned vehicle calculates the current predicted pose state corresponding to the unmanned vehicle based on the current observation positioning result, the current Kalman gain matrix and the time update pose state corresponding to the unmanned vehicle, namely, the current predicted pose state corresponding to the unmanned vehicle at the current moment is predicted based on an EKF (Extended Kalman Filter) frame, the unmanned vehicle can accurately position the pose of the unmanned vehicle in a complex port environment, and the safety and the working stability of the unmanned vehicle in the working process can be ensured.
The positioning device of the unmanned vehicle comprises a processor and a memory, wherein the first acquisition unit, the first calculation unit, the second acquisition unit, the third acquisition unit, the second calculation unit, the third calculation unit and the like are stored in the memory as program units, and the processor executes the program units stored in the memory to realize corresponding functions.
The processor comprises a kernel, and the kernel calls the corresponding program unit from the memory. The kernel can be set to be one or more than one, and the pose of the unmanned automobile is accurately positioned by adjusting the kernel parameters, so that a positioning result with higher accuracy is obtained.
The memory may include volatile memory in a computer readable medium, random Access Memory (RAM) and/or nonvolatile memory such as Read Only Memory (ROM) or flash memory (flash RAM), and the memory includes at least one memory chip.
An embodiment of the present invention provides a storage medium, on which a program is stored, where the program, when executed by a processor, implements the method for positioning an unmanned vehicle described in any one of the above embodiments.
An embodiment of the present invention provides a processor, where the processor is configured to run a program, where the program executes the positioning method for an unmanned vehicle according to any one of the above embodiments when running.
The embodiment of the invention provides equipment, which comprises a processor, a memory and a program which is stored on the memory and can run on the processor, wherein the processor executes the program and realizes the following steps:
acquiring a current control quantity, a historical observation pose state, a historical prediction pose state and a historical prediction covariance matrix, and calculating a time update pose state and a time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix;
acquiring a current observation positioning result;
acquiring a current observation matrix and a current observation covariance matrix, substituting the current observation matrix, the current observation covariance matrix and the time updating covariance matrix into a first preset algorithm, and calculating the current Kalman gain matrix;
and substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate the current prediction pose state.
Further, the calculating a time update pose state and a time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state, and the historical prediction covariance matrix includes:
performing partial derivation processing on the historical observation pose state by using a preset function to obtain a first Jacobian matrix corresponding to the historical observation pose state;
performing partial derivative processing on the current control quantity by using the preset function to obtain a second Jacobian matrix corresponding to the current control quantity;
substituting the first Jacobian matrix, the second Jacobian matrix, the current control quantity and the historical predicted pose state into a third preset algorithm, and calculating the time update pose state;
and substituting the first Jacobian matrix, the second Jacobian matrix, the historical prediction covariance matrix and the custom generation number into a fourth preset algorithm, and calculating the time update covariance matrix.
Further, the obtaining of the current observation positioning result includes:
substituting the time updating pose state into a fifth preset algorithm, and calculating a current prediction positioning result;
obtaining a first current positioning result, and calculating a first difference value between the first current positioning result and the current prediction positioning result;
if the first difference is smaller than or equal to a preset threshold vector, determining the first current positioning result as the current observation positioning result;
if the first difference is larger than the preset threshold vector, obtaining a second current positioning result, and calculating a second difference between the second current positioning result and the current predicted positioning result;
if the second difference is smaller than or equal to the preset threshold vector, determining the second current positioning result as the current observation positioning result;
if the second difference is larger than the preset threshold vector, acquiring a third current positioning result, and calculating a third difference between the third current positioning result and the current predicted positioning result;
if the third difference is smaller than or equal to the preset threshold vector, determining the third current positioning result as the current observation positioning result;
if the third difference is greater than the preset threshold vector, determining a current positioning result corresponding to the minimum value of the first difference, the second difference and the third difference as the current observation positioning result; the first current positioning result is a first positioning result obtained based on a differential GPS positioning method, a laser positioning method and a visual positioning method, the second current positioning result is a second positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method, and the third current positioning result is a third positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method.
Further, after the time update pose state, the current kalman gain matrix, and the current observation positioning result are substituted into a second preset algorithm to calculate a current prediction pose state, the method further includes:
substituting the current observation matrix, the current Kalman gain matrix and the time update covariance matrix into a sixth preset algorithm to calculate a current prediction covariance matrix;
and caching the current prediction pose state, the current prediction covariance matrix and the current observation positioning result into a local storage space.
Further, after the time update pose state, the current kalman gain matrix, and the current observation positioning result are substituted into a second preset algorithm to calculate a current predicted pose state, the method further includes:
and outputting and displaying the current predicted pose state.
The present application further provides a computer program product adapted to perform program code for initializing the following method steps when executed on a data processing device: acquiring a current control quantity, a historical observation pose state, a historical prediction pose state and a historical prediction covariance matrix, and calculating a time update pose state and a time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix; acquiring a current observation positioning result; acquiring a current observation matrix and a current observation covariance matrix, substituting the current observation matrix, the current observation covariance matrix and the time update covariance matrix into a first preset algorithm, and calculating the current Kalman gain matrix; and substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate the current prediction pose state.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
In a typical configuration, a computing device includes one or more processors (CPUs), input/output interfaces, network interfaces, and memory.
The memory may include forms of volatile memory in a computer readable medium, random Access Memory (RAM) and/or non-volatile memory, such as Read Only Memory (ROM) or flash memory (flash RAM). The memory is an example of a computer-readable medium.
Computer-readable media, including both non-transitory and non-transitory, removable and non-removable media, may implement information storage by any method or technology. The information may be computer readable instructions, data structures, modules of a program, or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static Random Access Memory (SRAM), dynamic Random Access Memory (DRAM), other types of Random Access Memory (RAM), read Only Memory (ROM), electrically Erasable Programmable Read Only Memory (EEPROM), flash memory or other memory technology, compact disc read only memory (CD-ROM), digital Versatile Disks (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices, or any other non-transmission medium, which can be used to store information that can be accessed by a computing device. As defined herein, a computer readable medium does not include a transitory computer readable medium such as a modulated data signal and a carrier wave.
It should also be noted that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrases "comprising a," "8230," "8230," or "comprising" does not exclude the presence of additional identical elements in the process, method, article, or apparatus comprising the element.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The above are merely examples of the present application and are not intended to limit the present application. Various modifications and changes may occur to those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.

Claims (7)

1. A method of locating an unmanned vehicle, comprising:
acquiring a current control quantity, a historical observation pose state, a historical prediction pose state and a historical prediction covariance matrix, and calculating a time update pose state and a time update covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix;
acquiring a current observation positioning result;
acquiring a current observation matrix and a current observation covariance matrix, substituting the current observation matrix, the current observation covariance matrix and the time updating covariance matrix into a first preset algorithm, and calculating a current Kalman gain matrix;
substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate a current prediction pose state;
the calculating the time updating pose state and the time updating covariance matrix according to the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix comprises the following steps:
performing partial derivation processing on the historical observation pose state by using a preset function to obtain a first Jacobian matrix corresponding to the historical observation pose state;
performing partial derivative processing on the current control quantity by using the preset function to obtain a second Jacobian matrix corresponding to the current control quantity;
substituting the first Jacobian matrix, the second Jacobian matrix, the current control quantity and the historical predicted pose state into a third preset algorithm, and calculating the time update pose state;
substituting the first Jacobian matrix, the second Jacobian matrix, the historical prediction covariance matrix and the number of user-defined generations into a fourth preset algorithm, and calculating the time update covariance matrix;
the obtaining of the current observation positioning result includes:
substituting the time updating pose state into a fifth preset algorithm, and calculating a current prediction positioning result;
obtaining a first current positioning result, and calculating a first difference value between the first current positioning result and the current prediction positioning result;
if the first difference is smaller than or equal to a preset threshold vector, determining the first current positioning result as the current observation positioning result;
if the first difference is larger than the preset threshold vector, obtaining a second current positioning result, and calculating a second difference between the second current positioning result and the current predicted positioning result;
if the second difference is smaller than or equal to the preset threshold vector, determining the second current positioning result as the current observation positioning result;
if the second difference is larger than the preset threshold vector, obtaining a third current positioning result, and calculating a third difference between the third current positioning result and the current predicted positioning result;
if the third difference is smaller than or equal to the preset threshold vector, determining the third current positioning result as the current observation positioning result;
if the third difference is greater than the preset threshold vector, determining a current positioning result corresponding to the minimum value of the first difference, the second difference and the third difference as the current observation positioning result; the first current positioning result is a positioning result obtained based on a differential GPS positioning method, the second current positioning result is a positioning result obtained based on a laser positioning method, and the third current positioning result is a positioning result obtained based on a visual positioning method.
2. The method of claim 1, wherein after said calculating a current predicted pose state by substituting said time-updated pose state, said current kalman gain matrix, and said current observed position fix into a second predetermined algorithm, said method further comprises:
substituting the current observation matrix, the current Kalman gain matrix and the time updating covariance matrix into a sixth preset algorithm to calculate a current prediction covariance matrix;
and caching the current prediction pose state, the current prediction covariance matrix and the current observation positioning result into a local storage space.
3. The method according to any one of claims 1-2, characterized in that after said calculating a current predicted pose state by substituting the time-updated pose state, the current kalman gain matrix, and the current observation position fix into a second preset algorithm, the method further comprises:
and outputting and displaying the current predicted pose state.
4. A positioning device for an unmanned vehicle, comprising:
the first acquisition unit is used for acquiring the current control quantity, the historical observation pose state, the historical prediction pose state and the historical prediction covariance matrix;
a first calculation unit, configured to calculate a time update pose state and a time update covariance matrix according to the current control amount, the historical observation pose state, the historical prediction pose state, and the historical prediction covariance matrix;
the second acquisition unit is used for acquiring a current observation positioning result;
a third obtaining unit, configured to obtain a current observation matrix and a current observation covariance matrix;
the second calculation unit is used for substituting the current observation matrix, the current observation covariance matrix and the time updating covariance matrix into a first preset algorithm to calculate the current Kalman gain matrix;
the third calculation unit is used for substituting the time updating pose state, the current Kalman gain matrix and the current observation positioning result into a second preset algorithm to calculate a current prediction pose state;
the first partial derivative module is used for performing partial derivative processing on the historical observation pose state by using a preset function so as to obtain a first Jacobian matrix corresponding to the historical observation pose state;
the second partial derivative module is used for performing partial derivative processing on the current control quantity by using the preset function so as to obtain a second Jacobian matrix corresponding to the current control quantity;
the first calculation module is used for substituting the first Jacobian matrix, the second Jacobian matrix, the current control quantity and the historical predicted pose state into a third preset algorithm to calculate the time updating pose state;
the second calculation module is used for substituting the first Jacobian matrix, the second Jacobian matrix, the historical prediction covariance matrix and the custom generation number into a fourth preset algorithm to calculate the time update covariance matrix;
the second acquisition unit includes:
the third calculation module is used for substituting the time updating pose state into a fifth preset algorithm and calculating a current prediction positioning result;
the fourth calculation module is used for acquiring a first current positioning result and calculating a first difference value between the first current positioning result and the current predicted positioning result;
a first determining module, configured to determine the first current positioning result as the current observation positioning result when the first difference is smaller than or equal to a preset threshold vector;
a fifth calculating module, configured to obtain a second current positioning result when the first difference is greater than the preset threshold vector, and calculate a second difference between the second current positioning result and the current predicted positioning result;
a second determining module, configured to determine the second current positioning result as the current observed positioning result when the second difference is smaller than or equal to the preset threshold vector;
a sixth calculating module, configured to obtain a third current positioning result when the second difference is greater than the preset threshold vector, and calculate a third difference between the third current positioning result and the current predicted positioning result;
a third determining module, configured to determine the third current positioning result as the current observed positioning result when the third difference is smaller than or equal to the preset threshold vector;
a fourth determining module, configured to determine, when the third difference is greater than the preset threshold vector, a current positioning result corresponding to a minimum value of the first difference, the second difference, and the third difference as the current observation positioning result; the first current positioning result is a first positioning result obtained based on a differential GPS positioning method, a laser positioning method and a visual positioning method, the second current positioning result is a second positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method, and the third current positioning result is a third positioning result obtained based on the differential GPS positioning method, the laser positioning method and the visual positioning method.
5. The apparatus of claim 4, wherein the first computing unit comprises:
the first partial derivative module is used for performing partial derivative processing on the historical observation pose state by using a preset function so as to obtain a first Jacobian matrix corresponding to the historical observation pose state;
the second partial derivative module is used for performing partial derivative processing on the current control quantity by using the preset function so as to obtain a second Jacobian matrix corresponding to the current control quantity;
the first calculation module is used for substituting the first Jacobian matrix, the second Jacobian matrix, the current control quantity and the historical predicted pose state into a third preset algorithm to calculate the time updating pose state;
and the second calculation module is used for substituting the first Jacobian matrix, the second Jacobian matrix, the historical prediction covariance matrix and the custom generation number into a fourth preset algorithm to calculate the time update covariance matrix.
6. A storage medium, characterized in that the storage medium comprises a stored program, wherein the program, when executed, controls an apparatus in which the storage medium is located to perform the method for locating an unmanned vehicle according to any one of claims 1 to 3.
7. A processor, characterized in that the processor is configured to run a program, wherein the program when executed performs the method of locating an unmanned vehicle of any of claims 1 to 3.
CN201910314074.5A 2019-04-18 2019-04-18 Positioning method and device for unmanned vehicle Active CN109947116B (en)

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 CN109947116A (en) 2019-06-28
CN109947116B true 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)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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

Citations (3)

* Cited by examiner, † Cited by third party
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
CN107193026A (en) * 2017-05-06 2017-09-22 千寻位置网络有限公司 Pseudorange positioning smooth method and system, positioning terminal

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102706342A (en) * 2012-05-31 2012-10-03 重庆邮电大学 Location and environment modeling method of intelligent movable robot
FR3000813B1 (en) * 2013-01-04 2016-04-15 Parrot ROTARY SAILING DRONE COMPRISING MEANS FOR AUTONOMOUS POSITION DETERMINATION IN AN ABSOLUTE FLOOR - RELATED MARK.
CN107426693B (en) * 2017-06-01 2020-12-01 北京星选科技有限公司 Positioning method and device
CN109141427B (en) * 2018-08-29 2022-01-25 上海理工大学 EKF positioning method based on distance and angle probability model under non-line-of-sight environment
CN109490931A (en) * 2018-09-03 2019-03-19 天津远度科技有限公司 Flight localization method, device and unmanned plane

Patent Citations (3)

* Cited by examiner, † Cited by third party
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
CN107193026A (en) * 2017-05-06 2017-09-22 千寻位置网络有限公司 Pseudorange positioning smooth method and system, positioning terminal

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Adaptive_wavelet_packet_thresholding_with_iterative_Kalman_filter_for_speech_enhancement;Mengjiao Zhao;《2017 IEEE Global Conference on Signal and Information Processing》;20180408;全文 *
基于双目立体视觉的船舶轨迹跟踪算法研究;黄椰;《计算机科学》;20170115;全文 *

Also Published As

Publication number Publication date
CN109947116A (en) 2019-06-28

Similar Documents

Publication Publication Date Title
KR102518532B1 (en) Apparatus for determining route of autonomous vehicle and method thereof
CN110262486B (en) Unmanned equipment motion control method and device
JP6643883B2 (en) GPS drift processing method and system
CN109947116B (en) Positioning method and device for unmanned vehicle
CN110246182A (en) Vision-based global map positioning method and device, storage medium and equipment
US20230341240A1 (en) Method and processor circuit for updating a digital road map
CN110059095B (en) Data updating method and device
CN110103987B (en) Decision planning method and device applied to automatic driving vehicle
CN110956214B (en) Training method and device for automatic driving vision positioning model
CN110239553B (en) Method and device for calculating virtual accelerator pedal opening of electric automobile
JP2023171384A (en) Avionics system, aircraft, and storage medium
CN108240807B (en) Method for estimating space occupation
CN112729349B (en) Method and device for on-line calibration of odometer, electronic equipment and storage medium
CN109459769A (en) A kind of autonomic positioning method and system
CN111568304B (en) Sweeping robot positioning method and device and sweeping robot
Nalla Perumal et al. Sensor fusion for automotive dead reckoning using GPS and IMU for accurate position and velocity estimation
CN113449433A (en) Constraint optimization method and device for objective function corresponding to cement production process model
CN106842927B (en) Control parameter adjusting method and device and double-wheel self-balancing robot
CN116105731B (en) Navigation method and device under sparse ranging condition, computer equipment and medium
CN113492863B (en) Automatic driving control method and device
CN112819861B (en) Point cloud motion compensation method, device and computer readable storage medium
CN116331255A (en) Automatic driving transverse control method and device
CN118306404A (en) Method and device for determining navigation position of vehicle
CN117889888A (en) Nonlinear initial alignment method based on speed error transformation
Nattharith Implementation of Cooperative Sub-systems for Mobile Robot Navigation

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