CN111811503B - Unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code - Google Patents
Unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code Download PDFInfo
- Publication number
- CN111811503B CN111811503B CN202010678319.5A CN202010678319A CN111811503B CN 111811503 B CN111811503 B CN 111811503B CN 202010678319 A CN202010678319 A CN 202010678319A CN 111811503 B CN111811503 B CN 111811503B
- Authority
- CN
- China
- Prior art keywords
- equation
- vehicle
- state
- dimensional code
- ultra
- 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
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Abstract
The invention discloses an unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional codes, which comprises the steps of obtaining a state updating equation of a vehicle pose based on a kinematics equation of a front fork type automatic guided vehicle; obtaining a measurement information equation of the vehicle pose through ultra-wideband positioning, two-dimensional code positioning, a gyroscope and an encoder; obtaining sigma points and sigma weights through UT conversion based on the state updating equation and the measurement information equation; calculating Kalman gain based on a state matrix cross-correlation function, and updating a state and covariance; designing a fuzzy inference system based on the characteristics of ultra wide band and two-dimensional code positioning and the difference value in unscented Kalman filtering; and using the fuzzy inference system to adaptively adjust the process noise and the measurement noise of the estimation system. Therefore, the positioning precision, stability and instantaneity of the automatic guided vehicle are improved, and the requirement of a complex industrial environment is finally met.
Description
Technical Field
The invention relates to the technical field of Automatic Guided Vehicle (AGV) navigation and positioning, in particular to an unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional codes.
Background
With the continuous penetration of the artificial intelligence and internet mode, the heavy work of the robot replacing human gradually tends to be great, and the AGV is used as an important branch of the intelligent mobile robot, so that the intelligent mobile robot is safer and more stable and has great engineering significance and research value. Around the positioning problem of the AGV, scholars at home and abroad make a lot of research works, such as laser positioning, visual positioning, inertial positioning, etc., and these positioning modes can obtain good positioning effects for different working scenes and cost. At present, a plurality of problems still exist in the positioning of a single sensor: laser positioning is widely used for indoor positioning, but it requires expensive cost to achieve high-precision positioning in a complicated industrial environment. Two-dimensional code location has advantages such as positioning accuracy is high, maintenance convenience, however can only carry out coordinate calculation when scanning the two-dimensional code, can't be applicable to AGV whole navigation. In the case of UWB, in a complex industrial environment, the positioning signal is blocked, and then is transmitted and refracted, thereby causing non-line-of-sight errors.
Therefore, an unscented kalman filter fusion positioning method based on ultra wide band and two-dimensional code is urgently needed to be provided, so that the cost is reduced while the positioning accuracy, stability and instantaneity of the AGV in a complex industrial environment are improved.
Disclosure of Invention
The invention aims to provide an unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional codes, so as to improve the positioning precision of an automatic guided vehicle, reduce the layout cost to the maximum extent and finally enable the vehicle to meet the working requirement.
In order to achieve the purpose, the invention provides an unscented kalman filter fusion positioning method based on ultra wide band and two-dimensional code, comprising the following steps:
obtaining a state updating equation of the vehicle pose based on a kinematics equation of the front fork type automatic guide vehicle;
obtaining a measurement information equation of the vehicle pose through ultra-wideband positioning, two-dimensional code positioning, a gyroscope and an encoder;
obtaining sigma points and sigma weights through UT conversion based on the state updating equation and the measurement information equation;
calculating Kalman gain based on a state matrix cross-correlation function, and updating a state and covariance; the state matrix cross-correlation function is a cross-correlation function of a sigma point set in a state space and a measurement space;
designing a fuzzy inference system based on the characteristics of ultra wide band and two-dimensional code positioning and the difference value in unscented Kalman filtering; the difference between the ultra-wideband and two-dimensional code positioning characteristics and the unscented kalman filter can be expressed as follows:
QU_accuracy＝αUWB+αQR；
wherein QU _ accuracy is a value fed back based on the positioning characteristics, αUWBValue, alpha, fed back for ultra-wideband tagsUWBIs 0 to 30, alphaQRDifference is a value fed back by the camera and represents a difference between the observed value and the estimated value, zx,zyIndicating the X, Y coordinates measured by the sensor,
representing X, Y coordinates obtained by a state update equation;
and using the fuzzy inference system to adaptively adjust the process noise and the measurement noise of the estimation system.
The state updating equation of the vehicle pose is obtained based on the kinematics equation of the front fork type automatic guide vehicle, and the state updating equation specifically comprises the following steps:
establishing an angular velocity state updating equation and a position state updating equation of the vehicle;
and establishing a state updating equation of the vehicle pose based on different yaw rates of the steering wheel according to the angular velocity state updating equation and the position state updating equation.
The state updating equation of the vehicle pose is obtained based on a kinematic equation of the front fork type automatic guide vehicle, wherein the state updating equation is as follows:
wherein X (delta t) is the pose offset of the vehicle at the delta t moment, g is a vehicle pose state update equation, w is the yaw velocity of the vehicle, and w isdIs the yaw rate of the steering wheel, theta is the yaw angle of the vehicle, v is the linear velocity of the vehicle,
as an offset in the x-coordinate,
as an offset in the y-coordinate,
the offset of the yaw angle, X (t), and Y (t) are the X and Y coordinates of the position of the vehicle at time t.
Wherein, obtain the measurement information equation of vehicle position appearance through ultra wide band location, two-dimensional code location, gyroscope and encoder, specifically include:
measuring position information data of the vehicle by an ultra-wideband, scanning the two-dimensional code by a camera to obtain pose information data of the vehicle, measuring yaw angle information data of the vehicle by a gyroscope, and measuring angular velocity and linear velocity information data of a steering wheel by an encoder;
and establishing a measurement information equation of the vehicle pose according to the position information data of the vehicle, the pose information data of the vehicle, the yaw angle information data of the vehicle and the angular velocity and linear velocity information data of the steering wheel.
The measurement information equation of the vehicle pose is obtained through ultra-wideband positioning, two-dimensional code positioning, a gyroscope and an encoder, wherein the measurement information equation is as follows:
wherein, at time t, θqThe yaw angle is calculated by scanning the two-dimensional code by a camera; xq，YqRespectively absolute coordinates obtained by calculating two scanning positions of a camera; xu，YuRespectively calculating the coordinates of the ultra-wide band tag; thetaeIs the yaw angle acquired by the gyroscope.
Obtaining sigma points and sigma weights through UT transformation based on the state updating equation and the measurement information equation, wherein the UT transformation specifically comprises the following steps:
wherein the content of the first and second substances,
and
respectively updating the sigma point and the sigma weight value after the estimation state is transformed by UT,
and PzRespectively updating sigma point and sigma weight, chi, for the measurement information after UT conversionk-1And wk-1Respectively sigma point and sigma weight calculated by the state quantity, and Q and R respectively represent process noise and measurement noise.
The Kalman gain is calculated based on a state matrix cross-correlation function, state updating and covariance updating are carried out, and the calculation formula can be expressed as follows:
wherein, TxzAs a cross-correlation function of the state matrix, KkIn order to be the basis of the kalman gain,
for updated state, PkFor the updated state covariance, T is the matrix transpose symbol.
The method for adaptively adjusting the process noise and the measurement noise of the estimation system by using the fuzzy inference system comprises the following steps:
Qk＝Qk-1×αQ,Rk＝Rk-1×αR；
wherein alpha isQ，αRRespectively outputting process noise and measurement noise coefficients for the fuzzy inference system; qk-1，Rk-1Process noise and measurement noise at the previous time; qk，RkFor process noise and measurement noise at the present time, and QkHas a value range of 0.5 to 3, RkThe value range of (A) is 0.5-15.
The invention relates to an unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional codes.A state update equation of a vehicle pose is obtained through a kinematics equation based on a front fork type automatic guided vehicle; obtaining a measurement information equation of the vehicle pose through ultra-wideband positioning, two-dimensional code positioning, a gyroscope and an encoder; obtaining sigma points and sigma weights through UT conversion based on the state updating equation and the measurement information equation; calculating Kalman gain based on a state matrix cross-correlation function, and updating a state and covariance; designing a fuzzy inference system based on the characteristics of ultra wide band and two-dimensional code positioning and the difference value in unscented Kalman filtering; and using the fuzzy inference system to adaptively adjust the process noise and the measurement noise of the estimation system. Therefore, the positioning precision, stability and instantaneity of the automatic guided vehicle are improved, and the requirement of a complex industrial environment is finally met.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a schematic flow chart of an unscented Kalman filter fusion positioning method based on ultra wide band and two-dimensional codes according to the present invention;
FIG. 2 is a flow chart of a fusion algorithm provided by the present invention;
FIG. 3 is a block diagram of an embodiment of the present invention;
FIG. 4 is an AGV kinematics model provided by the present invention;
FIG. 5 is an effect diagram of the unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code provided by the invention;
FIG. 6 is a comparison of the vehicle pose status provided by the present invention with recorded vehicle pose status;
FIG. 7 is a fuzzy control system membership function provided by the present invention.
Detailed Description
Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the drawings are illustrative and intended to be illustrative of the invention and are not to be construed as limiting the invention.
Referring to fig. 1, fig. 1 is a view illustrating an unscented kalman filter fusion positioning method based on an ultra-wideband and a two-dimensional code according to an embodiment of the present invention, specifically, the unscented kalman filter fusion positioning method based on an ultra-wideband and a two-dimensional code may include the following steps:
s101, obtaining a state updating equation of the vehicle pose based on a kinematics equation of the front fork type automatic guide vehicle;
referring to fig. 4, fig. 4 shows an AGV operation model according to an embodiment of the present invention. Specifically, an angular velocity state updating equation and a position state updating equation of the vehicle are established;
and establishing a state updating equation of the vehicle pose based on different yaw rates of the steering wheel according to the angular velocity state updating equation and the position state updating equation.
Wherein the state update equation is:
wherein X (delta t) is the pose offset of the vehicle at the delta t moment, g is a vehicle pose state update equation, w is the yaw velocity of the vehicle, and w isdIs the yaw rate of the steering wheel, theta is the yaw angle of the vehicle, v is the linear velocity of the vehicle,
as an offset in the x-coordinate,
as an offset in the y-coordinate,
the offset of the yaw angle, X (t), and Y (t) are the X and Y coordinates of the position of the vehicle at time t.
Where the steering wheel angle δ to yaw angle θ can be expressed as follows:
wherein v is vehicle linear velocity; l is the front and rear wheel base; δ is the steering angle of the steering wheel.
S102, obtaining a measurement information equation of the vehicle pose through ultra-wideband positioning, two-dimensional code positioning, a gyroscope and an encoder;
in the embodiment of the invention, the position information data of the vehicle is measured by an ultra-wideband, the position and attitude information data of the vehicle is obtained by scanning the two-dimensional code by a camera, the yaw angle information data of the vehicle is measured by a gyroscope, and the angular velocity and linear velocity information data of a steering wheel are measured by an encoder; the measurement interval of the position information measured by the ultra-wideband is 0.1 s/time, the position and pose information interval obtained by scanning the two-dimensional code by the camera is 0.05 s/time, the yaw angle information interval measured by the gyroscope is 0.1 s/time, and the angular velocity and linear velocity information interval of the steering wheel measured by the encoder is 0.05 s/time.
And establishing a measurement information equation of the vehicle pose according to the position information data of the vehicle, the pose information data of the vehicle, the yaw angle information data of the vehicle and the angular velocity and linear velocity information data of the steering wheel. Wherein, the measurement information equation is as follows:
wherein, at time t, θqThe yaw angle is calculated by scanning the two-dimensional code by a camera; xq，YqRespectively absolute coordinates obtained by calculating two scanning positions of a camera; xu，YuRespectively calculating the coordinates of the ultra-wide band tag; thetaeIs the yaw angle acquired by the gyroscope.
S103, obtaining sigma points and sigma weights through UT conversion based on the state updating equation and the measurement information equation;
in the embodiment of the present invention, in the above scheme, the sigma point and the sigma weight need to be obtained through UT transformation. The UT transform specifically includes:
wherein the content of the first and second substances,
and
respectively updating the sigma point and the sigma weight value after the estimation state is transformed by UT,
and PzRespectively updating sigma point and sigma weight, chi, for the measurement information after UT conversionk-1And wk-1Respectively calculating a sigma point and a sigma weight value obtained by state quantity, and respectively obtaining process noise and measurement noise by Q and R. In the UT conversion, 2n +1 sigma points are generated by the number n of states, and the mean value and the weight calculation formula of the sigma point set are as follows:
wherein mu is the mean value of the state, lambda is a hyperparameter, the larger the value is, the more the sigma point deviates from the mean value of the state, the predicted value can be obtained by substituting the mean value of the sigma point into the state transfer function, and then a new mean value and equation are calculated by the mean value and weight of the sigma point set, and the calculation formula is as follows:
wherein xk+1Representing the predicted value, Sk+1The value obtained by the measurement equation is represented.
S104, calculating Kalman gain based on a state matrix cross-correlation function, and updating a state and covariance;
in the embodiment of the invention, the cross-correlation function of the sigma point set in the state space and the measurement space is calculated, and the calculation formula is as follows:
wherein T isxzThe cross-correlation function in state space and measurement space for 2n +1 sigma point sets,
is the weight of the sigma point and,
and
the values of the estimation state and the measurement state are respectively calculated according to the updating step of the Kalman filtering based on the values, and the calculation formula of the Kalman filtering is as follows:
and finally, making an estimation of the state and an update equation of the state covariance matrix, wherein the formula is as follows:
wherein, TxzAs a cross-correlation function of the state matrix, KkIn order to be the basis of the kalman gain,
to be updatedState, PkFor the updated state covariance, T is the matrix transpose symbol.
S105, designing a fuzzy inference system based on the characteristics of ultra-wideband and two-dimensional code positioning and the difference value in unscented Kalman filtering;
in the embodiment of the invention, in the scheme, the numerical value of the accuracy QU _ accuracycacy needs to be calculated in real time based on the UWB and the two-dimensional code positioning characteristics, and the difference value between the measured value and the estimated value in the UKF needs to be calculated in real time.
The specific method for calculating the accuracy value is as follows:
QU_accuracy＝αUWB+αQR； (17)
wherein QU _ accuracy is a value fed back based on positioning characteristics, αUWBThe value of the feedback value of the UWB tag is related to factors influencing positioning accuracy, such as the distance between the tag and a base station, the distribution of the base station and the like, and the value range is 0-30. Alpha is alphaQRThe value fed back by the camera is related to the state of the scanned two-dimensional code, the accuracy of a two-dimensional code positioning frame, the real-time vehicle speed and other factors influencing the positioning of the two-dimensional code are related, and the value range is 0-70. The specific calculation rule is as follows: whether the distance value from the UWB tag to each base station is changed greatly (0-10), whether the base station distribution is rectangular distribution (0-10) and the distance from the UWB tag to the edge position is (0-10); the method comprises the steps of scanning a two-dimensional code, wherein the scanning step comprises the steps of scanning a difference value (0-10) between a two-dimensional code positioning frame and a standard value, scanning a difference value (0-10) between three positioning feature frames of the two-dimensional code and the standard value, scanning a difference value (0-10) between a connecting line of the three positioning feature frames of the two-dimensional code and a regular triangle, scanning whether the three positioning feature frames (0-10) are scanned, identifying whether the two-dimensional code coding content (0-10) is available, and continuously judging whether the three-time positioning data are changed greatly (-50-10).
QU accuracycacy can be calculated in real time in the area of travel based on the above calculation rules. And the difference represents the difference value between the observed value and the estimated value, and the noise parameter of the UKF is output through a fuzzy inference system.
The specific rules can be seen in table 1 and the membership function graph in figure 7.
TABLE 1 fuzzy control System fuzzy rules
And S106, using the fuzzy inference system to adaptively adjust the process noise and the measurement noise of the estimation system.
In the embodiment of the invention, the mode of adaptively adjusting the process noise and the measurement noise of the estimation system by using the fuzzy inference system is as follows:
Qk＝Qk-1×αQ,Rk＝Rk-1×αR(ii) a Wherein alpha isQ，αRRespectively outputting process noise and measurement noise coefficients for the fuzzy inference system; qk-1，Rk-1Process noise and measurement noise at the previous time; qk，RkFor process noise and measurement noise at the present time, and QkHas a value range of 0.5 to 3, RkThe value range of (A) is 0.5-15.
The invention is further illustrated by the following concrete simulation examples:
in order to effectively verify the positioning effect of the algorithm, the simulation test is carried out on the vehicle by using the measurement information of the analog sensor, firstly, the measurement information of a UWB, a camera, a gyroscope and an encoder is replaced by using the analog information, the UWB adopts an analog signal with the noise of +/-0.3 m, the camera adopts an analog signal with the noise of +/-0.3 m, the gyroscope adopts an analog signal with the noise of +/-0.1 m, and the encoder adopts an analog signal with the noise of +/-0.1 m. And (3) sequentially inputting the information of each sensor as measurement information into a UKF algorithm, wherein the specific process is as follows:
and inputting a camera positioning result if the two-dimensional code is scanned, and inputting a UWB positioning result if the two-dimensional code is not scanned. The encoder and gyroscope signals are input in real time. And outputting a UKF noise parameter through a fuzzy inference system based on the accuracy parameter detected by the characteristics and the difference value between the measured value and the observed value to dynamically adjust the noise. And estimating the pose of the automobile by using an algorithm. Firstly, setting the road adhesion coefficient to be 0.8, enabling the vehicle to run at a constant speed of 0.5m/S, and detecting and recording the pose state of the vehicle every 0.05S. After the vehicle has completed the prescribed route, the sensor measurement signals are simulated based on the measurement times of the individual sensors and the sensor noise distribution.
Then, in order to test the performance of the method provided by the invention, the sensor signals are input into the vehicle pose state obtained by the traditional UKF algorithm, the vehicle pose state obtained by the algorithm added into the fuzzy inference system is compared with the recorded vehicle pose state, and the result is shown in FIGS. 5 and 6. Observing fig. 5, it can be found that the provided method can effectively reduce the measurement error of the sensor, and the whole positioning effect is better. Observing fig. 6, it can be seen that the AGV can reach higher accuracy in a place where the two-dimensional code exists, and simultaneously, the error of the UWB caused by the non-line-of-sight error can be reduced by arranging the two-dimensional code on the driving route. The unscented Kalman filtering fusion positioning method based on the ultra wide band and the two-dimensional code can effectively improve the positioning precision of the AGV in time, and when the vehicle is in an occasion needing high precision, the accurate positioning is realized through the two-dimensional code positioning. Meanwhile, non line of sight (NLOS) positioning errors caused by UWB environmental problems are greatly reduced, and the method has the advantages of convenience in site arrangement and maintenance, higher positioning accuracy, higher stability and the like.
The method is based on an unscented Kalman filtering algorithm, and a state updating equation of the vehicle pose is obtained by establishing a kinematics equation based on a front fork type automatic guided vehicle; receiving ultra-wideband (UWB) positioning, two-dimensional code positioning and position and pose information of a gyroscope through a serial port and forming a measurement information equation of the vehicle position and pose; estimating an optimal pose by an unscented Kalman filtering algorithm based on a state updating equation and a measurement information equation; the noise of the algorithm is dynamically adjusted through a fuzzy inference system; fig. 3 is a structure diagram of an actual vehicle system, wherein a main controller is composed of an algorithm chip and a control chip, a UWB module, a camera module and a gyroscope transmit data to the algorithm chip through a serial port, a required rotation angle and a required forward speed are calculated through an algorithm, and then the rotation angle and the forward speed are transmitted to the control chip to control a servo driver, and a WIFI module and a PC terminal are used for controlling; fig. 2 is a flow chart of a fusion algorithm, so that the positioning accuracy, stability and real-time performance of the automatic guided vehicle are improved, and the requirement of a complex industrial environment is finally met.
While the invention has been described with reference to a preferred embodiment, it will be understood by those skilled in the art that various changes in form and detail may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.
Claims (8)
1. An unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional codes is characterized by comprising the following steps:
obtaining a state updating equation of the vehicle pose based on a kinematics equation of the front fork type automatic guide vehicle;
obtaining a measurement information equation of the vehicle pose through ultra-wideband positioning, two-dimensional code positioning, a gyroscope and an encoder;
obtaining sigma points and sigma weights through UT conversion based on the state updating equation and the measurement information equation;
calculating Kalman gain based on a state matrix cross-correlation function, and updating a state and covariance; the state matrix cross-correlation function is a cross-correlation function of a sigma point set in a state space and a measurement space;
designing a fuzzy inference system based on the characteristics of ultra wide band and two-dimensional code positioning and the difference value in unscented Kalman filtering; the difference between the ultra-wideband and two-dimensional code positioning characteristics and the unscented kalman filter can be expressed as follows:
QU_accuracy＝αUWB+αQR；
wherein QU _ accuracy is based on positioningValue of characteristic feedback, αUWBValue, alpha, fed back for ultra-wideband tagsUWBIs 0 to 30, alphaQRDifference is a value fed back by the camera and represents a difference between the observed value and the estimated value, zx,zyIndicating the X, Y coordinates measured by the sensor,
representing X, Y coordinates obtained by a state update equation;
and using the fuzzy inference system to adaptively adjust the process noise and the measurement noise of the estimation system.
2. The unscented kalman filter fusion positioning method based on ultra wide band and two-dimensional code of claim 1, characterized in that the state update equation of the vehicle pose is obtained based on the kinematics equation of the front fork type automated guided vehicle, which specifically includes:
establishing an angular velocity state updating equation and a position state updating equation of the vehicle;
and establishing a state updating equation of the vehicle pose based on different yaw rates of the steering wheel according to the angular velocity state updating equation and the position state updating equation.
3. The ultra-wideband and two-dimensional code based unscented kalman filter fusion positioning method of claim 2, characterized in that the state update equation of the vehicle pose is obtained based on the kinematics equation of the front fork type automated guided vehicle, wherein the state update equation is:
wherein X (delta t) is the pose offset of the vehicle passing delta t moment, and g is the pose state of the vehicleNew equation, w is yaw rate of vehicle, wdIs the yaw rate of the steering wheel, theta is the yaw angle of the vehicle, v is the linear velocity of the vehicle,
as an offset in the x-coordinate,
as an offset in the y-coordinate,
the offset of the yaw angle, X (t), and Y (t) are the X and Y coordinates of the position of the vehicle at time t.
4. The unscented kalman filter fusion positioning method based on ultra wide band and two-dimensional code of claim 1, characterized in that the measurement information equation of the vehicle pose is obtained through ultra wide band positioning, two-dimensional code positioning, gyroscope and encoder, which specifically includes:
measuring position information data of the vehicle by an ultra-wideband, scanning the two-dimensional code by a camera to obtain pose information data of the vehicle, measuring yaw angle information data of the vehicle by a gyroscope, and measuring angular velocity and linear velocity information data of a steering wheel by an encoder;
and establishing a measurement information equation of the vehicle pose according to the position information data of the vehicle, the pose information data of the vehicle, the yaw angle information data of the vehicle and the angular velocity and linear velocity information data of the steering wheel.
5. The ultra-wideband and two-dimensional code based unscented kalman filter fusion positioning method of claim 4, wherein the measurement information equation of the vehicle pose is obtained by ultra-wideband positioning, two-dimensional code positioning, a gyroscope and an encoder, wherein the measurement information equation is as follows:
wherein, at time t, θqThe yaw angle is calculated by scanning the two-dimensional code by a camera; xq，YqRespectively absolute coordinates obtained by calculating two scanning positions of a camera; xu，YuRespectively calculating the coordinates of the ultra-wide band tag; thetaeIs the yaw angle acquired by the gyroscope.
6. The ultra-wideband and two-dimensional code based unscented kalman filter fusion positioning method of claim 1, wherein the sigma point and the sigma weight are obtained by UT transformation based on the state update equation and the measurement information equation, wherein the UT transformation specifically comprises:
wherein the content of the first and second substances,
and
respectively updating the sigma point and the sigma weight value after the estimation state is transformed by UT,
and PzRespectively updating sigma point and sigma weight, chi, for the measurement information after UT conversionk-1And wk-1Respectively sigma point and sigma weight calculated by the state quantity, and Q and R respectively represent process noise and measurement noise.
7. The unscented kalman filter fusion positioning method based on ultra-wideband and two-dimensional codes according to claim 6, characterized in that kalman gain is calculated based on the state matrix cross-correlation function, and state update and covariance update are performed, and the calculation formula can be expressed as follows:
wherein, TxzAs a cross-correlation function of the state matrix, KkIn order to be the basis of the kalman gain,
for updated state, PkFor the updated state covariance, T is the matrix transpose symbol.
8. The ultra-wideband and two-dimensional code based unscented kalman filter fusion positioning method of claim 1, characterized in that the way of adaptively adjusting the process noise and measurement noise of the estimation system using the fuzzy inference system is as follows:
Qk＝Qk-1×αQ,Rk＝Rk-1×αR；
wherein alpha isQ，αRRespectively outputting process noise and measurement noise coefficients for the fuzzy inference system; qk-1，Rk-1Process noise and measurement noise at the previous time; qk，RkFor process noise and measurement noise at the present time, and QkHas a value range of 0.5 to 3, RkThe value range of (A) is 0.5-15.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010678319.5A CN111811503B (en) | 2020-07-15 | 2020-07-15 | Unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010678319.5A CN111811503B (en) | 2020-07-15 | 2020-07-15 | Unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111811503A CN111811503A (en) | 2020-10-23 |
| CN111811503B true CN111811503B (en) | 2022-02-18 |
Family
ID=72866117
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010678319.5A Active CN111811503B (en) | 2020-07-15 | 2020-07-15 | Unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111811503B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112533149B (en) * | 2020-11-27 | 2022-06-07 | 桂林理工大学 | Moving target positioning algorithm based on UWB mobile node |
| CN114623823B (en) * | 2022-05-16 | 2022-09-13 | 青岛慧拓智能机器有限公司 | UWB (ultra wide band) multi-mode positioning system, method and device integrating odometer |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9606217B2 (en) * | 2012-05-01 | 2017-03-28 | 5D Robotics, Inc. | Collaborative spatial positioning |
| CN104936148B (en) * | 2015-07-03 | 2018-04-27 | 中南大学 | A kind of WIFI indoor orientation methods based on fuzzy KNN |
| US9772395B2 (en) * | 2015-09-25 | 2017-09-26 | Intel Corporation | Vision and radio fusion based precise indoor localization |
| US20170336220A1 (en) * | 2016-05-20 | 2017-11-23 | Daqri, Llc | Multi-Sensor Position and Orientation Determination System and Device |
| CN107290688B (en) * | 2017-08-24 | 2019-08-09 | 合肥工业大学 | A kind of lithium battery SOC estimation method based on adaptive fuzzy Kalman filtering |
| CN109946731B (en) * | 2019-03-06 | 2022-06-10 | 东南大学 | Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering |
| CN109916410B (en) * | 2019-03-25 | 2023-04-28 | 南京理工大学 | Indoor positioning method based on improved square root unscented Kalman filtering |
| CN109974714B (en) * | 2019-04-29 | 2023-07-04 | 南京航空航天大学 | Sage-Husa self-adaptive unscented Kalman filtering attitude data fusion method |
| CN111152795B (en) * | 2020-01-08 | 2022-12-13 | 东南大学 | Model and parameter dynamic adjustment based adaptive vehicle state prediction system and prediction method |
-
2020
- 2020-07-15 CN CN202010678319.5A patent/CN111811503B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN111811503A (en) | 2020-10-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108955688B (en) | Method and system for positioning double-wheel differential mobile robot | |
| CN111811503B (en) | Unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code | |
| CN112882053B (en) | Method for actively calibrating external parameters of laser radar and encoder | |
| CN107167148A (en) | Synchronous superposition method and apparatus | |
| CN101221447A (en) | Mechanical automatic steering control method | |
| CN113359710B (en) | LOS theory-based agricultural machinery path tracking method | |
| CN110988894B (en) | Port environment-oriented multi-source data fusion unmanned automobile real-time positioning method | |
| CN106843214A (en) | A kind of tape guidance AGV tracking control methods based on Active Disturbance Rejection Control | |
| Wang et al. | Research on logistics autonomous mobile robot system | |
| CN110542429A (en) | Error compensation method for omnidirectional mobile robot | |
| CN112650217B (en) | Robot trajectory tracking strategy dynamic optimization method based on evaluation function | |
| CN116225029B (en) | Robot path planning method | |
| CN107861501A (en) | Underground sewage treatment works intelligent robot automatic positioning navigation system | |
| CN112683263B (en) | UWB/IMU/ODOM multi-sensor data fusion mobile robot positioning method based on improved model | |
| CN113580146B (en) | Mechanical arm real-time obstacle avoidance method integrating dynamic system and model predictive control | |
| CN113218384B (en) | Indoor AGV self-adaptive positioning method based on laser SLAM | |
| CN108107882B (en) | Automatic calibration and detection system of service robot based on optical motion tracking | |
| CN112489075B (en) | Sequential inertial multi-sensor fusion filtering method based on characteristic function | |
| CN110007680B (en) | Robot obstacle avoidance algorithm based on topological relation | |
| CN113203419A (en) | Indoor inspection robot correction positioning method based on neural network | |
| Ding et al. | A novel industrial AGV control strategy based on dual-wheel chassis model | |
| Tao | Research on intelligent robot patrol route based on cloud computing | |
| CN114088113B (en) | Odometer track alignment and precision evaluation method | |
| Doumbia et al. | A novel infrared navigational algorithm for autonomous robots | |
| CN117724504B (en) | Unmanned vehicle path tracking control method and device based on projection area |
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 | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20201023 Assignee: Liuzhou moling Technology Co.,Ltd. Assignor: GUILIN University OF ELECTRONIC TECHNOLOGY Contract record no.: X2022450000565 Denomination of invention: A fusion location method of unscented kalman filter based on uwb and 2d code Granted publication date: 20220218 License type: Common License Record date: 20221229 |
|
| EE01 | Entry into force of recordation of patent licensing contract |