CN115421153B - Laser radar and UWB combined positioning method and system based on extended Kalman filtering - Google Patents

Laser radar and UWB combined positioning method and system based on extended Kalman filtering Download PDF

Info

Publication number
CN115421153B
CN115421153B CN202210931857.XA CN202210931857A CN115421153B CN 115421153 B CN115421153 B CN 115421153B CN 202210931857 A CN202210931857 A CN 202210931857A CN 115421153 B CN115421153 B CN 115421153B
Authority
CN
China
Prior art keywords
uwb
time
positioning
agv
representing
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
CN202210931857.XA
Other languages
Chinese (zh)
Other versions
CN115421153A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202210931857.XA priority Critical patent/CN115421153B/en
Publication of CN115421153A publication Critical patent/CN115421153A/en
Application granted granted Critical
Publication of CN115421153B publication Critical patent/CN115421153B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Signal Processing (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

The embodiment of the invention discloses a laser radar and UWB combined positioning method and system based on extended Kalman filtering, which relate to the technical field of robot environment sensing and can further improve the positioning precision of an AGV in a complex environment. The invention comprises the following steps: the combined positioning model comprises: the AGV positioning system comprises a state model of the AGV and a measurement model established by the state model, wherein reflecting plates are arranged in a place where the AGV moves, a UWB positioning base station is stuck on each reflecting plate, and a UWB antenna and a laser radar form the AGV positioning system; receiving UWB pulse signals through a UWB antenna, receiving laser signals reflected by a reflector through a laser radar, and obtaining UWB positioning information and laser positioning information of the AGV according to the received signals; inputting the obtained UWB positioning information and laser positioning information into a combined positioning model, and updating an observation matrix; and carrying out iterative operation by using the updated observation matrix, and carrying out state prediction and measurement updating on the AGV.

Description

Laser radar and UWB combined positioning method and system based on extended Kalman filtering
Technical Field
The invention relates to the technical field of robot environment sensing, in particular to a laser radar and UWB combined positioning method and system based on extended Kalman filtering.
Background
The laser radar-based positioning method is used as one of AGV positioning modes, and is widely applied to industries such as warehouse logistics and the like in recent years. Positioning based on laser radar is realized mainly by a reflector with high reflection intensity and a three-point positioning method. According to the method, the reflector scanned by the laser radar at the current position is matched with the reflectors in the global map stored in advance, and as the coordinates of all reflectors in the environment are known, the global coordinates of the reflector scanned by the laser radar at the current position can be obtained according to the matching result, and when the obtained global coordinates are at least three, the current global coordinates of the laser radar can be obtained according to the distance between the laser radar and each reflector through a three-point positioning method.
However, the positioning algorithm has high requirements on the arrangement and detection of beacons, such as the artificial beacons in a scene cannot form symmetry or local similarity in space, and the number of the arrangement is required to ensure that the AGV can detect at least three beacons at any time in density, so that the minimum requirements of a three-point positioning method are met, and the like. However, in practical application, if the requirements cannot be met, the phenomena of mismatching, jamming or track missing and the like occur in the positioning or navigation process of the AGV, so that the accuracy is reduced. There is therefore a certain limitation to this positioning method. In order to improve the positioning accuracy of the AGV in a complex environment as much as possible under the premise of controlling the cost, how to improve the traditional laser radar positioning algorithm on the aspects of hardware and software algorithm so as to obtain a better positioning effect becomes a subject needing to be studied.
Disclosure of Invention
The embodiment of the invention provides a laser radar and UWB combined positioning method and system based on extended Kalman filtering, which can further improve the positioning accuracy of an AGV in a complex environment.
In order to achieve the above purpose, the embodiment of the present invention adopts the following technical scheme:
in a first aspect, an embodiment of the present invention provides a method, including:
s1, establishing a combined positioning model of an AGV, wherein the combined positioning model comprises: the system comprises a state model of an AGV and a measurement model established by using the state model, wherein a reflector is arranged in a place where the AGV moves, a UWB positioning base station is stuck on each reflector, and a positioning system of the AGV is formed by a UWB antenna and a laser radar;
s2, receiving UWB pulse signals through the UWB antenna, receiving laser signals reflected by the reflecting plate through the laser radar, and obtaining UWB positioning information and laser positioning information of the AGV according to the received signals, wherein the step S2 is periodically executed in the running process of the AGV;
s3, inputting the obtained UWB positioning information and laser positioning information into the combined positioning model, and updating an observation matrix;
s4, performing iterative operation by using the updated observation matrix through extended Kalman filtering, and performing state prediction and measurement updating on the AGV by using the iterative operation result.
In a second aspect, the positioning system provided by the embodiment of the invention is characterized in that the reflecting plates are arranged in a place where the AGV moves, a UWB positioning base station is stuck on each reflecting plate, and a UWB antenna and a laser radar form the positioning system of the AGV and are arranged on the AGV; the UWB antenna is used for receiving UWB pulse signals; the laser radar is used for receiving laser signals reflected by the reflecting plate; and the calculation module is arranged on the AGV and used for obtaining UWB positioning information and laser positioning information of the AGV according to the received signals.
The combined positioning model of AGV has been imported in the calculation module of installing on the AGV, the combined positioning model includes: the system comprises a state model of an AGV and a measurement model established by using the state model, wherein a reflector is arranged in a place where the AGV moves, a UWB positioning base station is stuck on each reflector, and a positioning system of the AGV is formed by a UWB antenna and a laser radar; the calculation module is specifically used for inputting the obtained UWB positioning information and laser positioning information into the combined positioning model and updating an observation matrix; and then carrying out iterative operation by using the updated observation matrix through extended Kalman filtering, and carrying out state prediction and measurement updating on the AGV by using the iterative operation result.
According to the laser radar and UWB combined positioning method and system based on the extended Kalman filtering, the UWB and the laser radar are utilized to form the double-sensor combined positioning system, and the distance constraint between the UWB/laser radar and a certain UWB is added in an observation equation, so that even if the situation that a triangle is congruent or similar to the other triangle in the global map or the local map exists in the reflector obtained by laser radar scanning at a certain moment, the distance between the UWB/laser radar and the certain identical UWB at the front moment and the rear moment is different, and therefore the risk of mismatching of the system can be further reduced theoretically, and the positioning precision of the AGV in a complex environment is further improved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIGS. 1-5 are schematic views of scenes in a specific example provided in an embodiment of the present invention;
FIG. 6 is a schematic diagram of an execution flow in a specific example provided in an embodiment of the present invention;
FIG. 7 is a schematic diagram of a system architecture according to an embodiment of the present invention;
fig. 8 is a schematic flow chart of a method according to an embodiment of the present invention.
Detailed Description
The present invention will be described in further detail below with reference to the drawings and detailed description for the purpose of better understanding of the technical solution of the present invention to those skilled in the art. Embodiments of the present invention will hereinafter be described in detail, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals refer to the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by referring to the drawings are exemplary only for explaining the present invention and are not to be construed as limiting the present invention. As used herein, the singular forms "a", "an", "the" and "the" are intended to include the plural forms as well, unless expressly stated otherwise, as understood by those skilled in the art. It will be further understood that the terms "comprises" and/or "comprising," when used in this specification, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof. It will be understood that when an element is referred to as being "connected" or "coupled" to another element, it can be directly connected or coupled to the other element or intervening elements may also be present. Further, "connected" or "coupled" as used herein may include wirelessly connected or coupled. The term "and/or" as used herein includes any and all combinations of one or more of the associated listed items. It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. It will be further understood that terms, such as those defined in commonly used dictionaries, should be interpreted as having a meaning that is consistent with their meaning in the context of the prior art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein.
The embodiment of the invention provides a laser radar and UWB combined positioning method based on extended Kalman filtering, which is shown in fig. 8 and comprises the following steps:
s1, establishing a combined positioning model of an AGV, wherein the combined positioning model comprises: the AGV comprises a state model of the AGV and a measurement model established by using the state model, wherein reflecting plates are arranged in a place where the AGV moves, a UWB positioning base station is stuck on each reflecting plate, and a positioning system of the AGV is formed by a UWB antenna and a laser radar. In this embodiment, by introducing UWB (Ultra Wide Band) and a laser radar to form a dual-sensor positioning system, the positioning result of the laser radar is subjected to state constraint and prediction correction by using an extended kalman filtering algorithm.
S2, receiving UWB pulse signals through the UWB antenna, receiving laser signals reflected by the reflecting plate through the laser radar, and obtaining UWB positioning information and laser positioning information of the AGV according to the received signals, wherein the step S2 is periodically executed in the running process of the AGV;
s3, inputting the obtained UWB positioning information and laser positioning information into the combined positioning model, and updating an observation matrix;
s4, performing iterative operation by using the updated observation matrix through extended Kalman filtering, and performing state prediction and measurement updating on the AGV by using the iterative operation result.
In this embodiment, in S1, it includes: establishing a state model X of an AGV k ＝A k X k-1 +w k-1 As a state equation of the UWB/laser radar combined positioning system, A k For locating the state transition matrix of the system, X k Is a state vector, w k-1 For process noise, k represents time; wherein, and->For the coordinate variation of the positioning system in x and y directions at time k with respect to time k-1,/for the positioning system>And->For the speed variation of the positioning system in x and y directions at time k with respect to time k-1. Specifically, in S2, the receiving, by the lidar, the laser signal reflected by the reflector includes: the laser radar emits laser signals to scan the reflecting plates arranged around the AGV, and the coordinate (x) of the laser radar at the kth moment is obtained through a triangulation method k ,y k )，x k 、y k Respectively representing coordinate parameters on two coordinate axes, and k represents time. For example: the main sensor is a laser radar, the auxiliary sensor is UWB, the laser radar is responsible for the main positioning task of the system, and the UWB is responsible for correcting when the laser radar fails. Therefore, the laser radar scans to obtain the coordinates of the surrounding reflector and obtains the coordinates (x k ,y k ) (in particular, this process can be implemented by a driver introduced into the lidar) the lidar derives coordinates (x) k-1 ,y k-1 ) So that
Is a combination of (a) and (b):
these parameters are all obtained from the coordinate data obtained by the lidar. Since the UWB is installed on each reflector and is arranged in the environment in advance, the coordinates of these UWB are known in this patent, e.g. the coordinates of the ith UWB are +.>
Specifically, w k-1 Satisfies a mean of 0 and a variance ofIs a gaussian distribution of (c); the period of step S2 is Δt, then:the measurement model established by the state model comprises the following components: y is Y k ＝C k X k +v k ，Y k For outputting matrix, v k To measure the noise matrix, C k Is an observation matrix;
wherein,ξ k，i in order to observe noise after Taylor expansion at time k, ζ k，k-1 To observe noise after Taylor expansion between k-1 and k time, specifically, ζ k，i Observing noise after Taylor expansion of the laser radar/UWB system at the moment k; zeta type toy k，k-1 Observation noise of the laser radar/UWB system after Taylor expansion between k-1 and k time can be understood as xi k，i Is observed noise, ζ, between lidar/UWB systems within the same time period k，k-1 Is the observed noise of the lidar/UWB system between different times. i represents the number of the reflector, d k，i Represents the distance between the positioning system and the UWB positioning base station on the ith reflector at time k, d k，k-1 Represents the distance between the position of the positioning system at the time k-1 and the position at the time k, d k0，i Representing the distance between the UWB/lidar system and the UWB on the ith reflector, d, at time k0 (i.e., the initial time of each calculation process) k0，k-1 Representing the distance between the UWB/lidar system at time k0 and time k;
x k0 x-coordinate, x, representing the system at the initial moment of each calculation process k-1 Representing the x coordinate, y of the system at time k-1 k0 Representing the y-coordinate, y of the system at the initial time of each calculation process k-1 Representing the y-coordinate of the system at time k-1，/>X-coordinate representing ith UWB, +.>Representing the y-coordinate of the ith UWB.
Distance between the positioning system and the UWB positioning base station on the ith reflector at time kThe distance between the position of the positioning system at time k-1 and the position at time k is +.>x k Representing the x coordinate, y of the system at time k k Representing the y-coordinate, v, of the system at time k k，i For the observed noise of the lidar/UWB system at time k (before taylor expansion, and ζ k，i From distinction), v k，k-1 For the observation noise of the lidar/UWB system between k-1 and k time (also before taylor expansion, and ζ k，k-1 Distinguishing from each other).
The iterative operation by the extended Kalman filter comprises the following steps: iterative operation is carried out through a recursive formula, wherein the recursive formula of an EKF (extended Kalman filter) algorithm is as follows:
P k ＝(I-K k C k )P′ k and takes initial value during iterative operationAnde denotes solving mathematical expectation (mean) operators, e.g. for X 0 The mean value is E [ X ] 0 ]；X 0 Representing the state quantity of the system at the initial moment; />Representing a state initial value of the system; p (P) 0 Mean square error is the initial state of the system; />An estimated value for the state quantity before correction; />The estimated value of the state quantity after the correction at the moment k; />The estimated value of the state quantity corrected at the moment k-1; k (K) k The Kalman gain of the system at the moment k is represented; p (P) k The mean square error of the state quantity corrected at the moment k; p (P) k-1 The mean square error of the state quantity corrected at the moment k-1; w (W) k Expanding a weight matrix for process noise Taylor; q (Q) k-1 Is the process noise variance; r's' k Mean square error for state quantity before correction; r is R k To observe the noise variance.
The step of performing state prediction and measurement update on the AGV by using the iterative operation result comprises the following steps: in each iteration, the prior estimation and the prior variance of the state quantity at the current moment are obtained through the iteration formula, wherein the coordinates of the AGV are used as the state input into the iteration formulaAn amount of; and acquiring the Kalman gain at the current moment by using the prior variance, and acquiring the posterior estimation by using the prior estimation and the Kalman gain. Specifically, the iterative operation process of the EKF algorithm includes two parts, namely state prediction and measurement update: first, system state quantity X k The initial value of (1) is taken asThe mean square error in the initial state is +.>
The EKF formula is as follows:
P k ＝(I-K k C K )P′ k (5)
wherein,an estimated value of the state quantity before correction, namely, a priori estimation of the state quantity; />The state quantity is an estimated value of the corrected state quantity, namely posterior estimation of the state quantity; p'. k The mean square error, namely the prior variance, of the state quantity before correction; p (P) k For corrected shapeThe state quantity mean square error is the post-test square difference; q (Q) k-1 Is the process noise variance; r is R k To observe the noise variance. Determination ofAnd->From equations (1) and (4), the a priori estimate of the state quantity at time k=1 can then be determined +.>And a priori variance P' 1 (the two previous calculations, known as state prediction, are predicted by a priori estimates and a priori variances of the state quantities) and then P' 1 To be entered (3) to obtain the kalman gain K at the moment of k=1 1 With K 1 And->Equation (2) allows the determination of +.1 at time k=1>I.e. estimation of the state quantity after correction at time k=1, i.e. a posterior estimation of the state quantity, with K 1 And P' 1 The state quantity mean square error P after the k=1 time correction can be obtained from the equation (5) 1 The latter operation is the continuous iteration of the previous process (the two calculations are i.e. measurement update, i.e. update is performed on the posterior estimation and posterior difference of the state quantity), the prior estimation and prior variance of the state quantity at the time of k=2 are obtained through the two operations of state prediction, the obtained result is substituted into the measurement update to obtain the posterior estimation and posterior difference of the state quantity at the time of k=2 through the two calculations, and k=3, 4, …, N and the like are obtained by continuous iteration update, and N is a positive integer. In short, the state quantity is the coordinates of the system, and the optimal value at each moment is obtained by continuously carrying out iterative operation through an EKF algorithm.
The state of the system is carried out through the above five iterative formulas (1) - (5)Prediction and metrology updates. The double-sensor combined positioning system is formed by utilizing UWB and laser radar, and distance constraint between UWB/laser radar and a certain UWB is added in an observation equation, so that even if a situation that a triangle is congruent or similar to another triangle in a global map or a local map exists in a reflector obtained by laser radar scanning at a certain moment, the distance between the UWB/laser radar and the same UWB at the front moment and the rear moment is different, and the risk of mismatching of the system can be further reduced theoretically. And predicting the state value through an extended Kalman filtering algorithm. Compared with the traditional Kalman filtering, the extended Kalman filtering has better prediction effect on a nonlinear system, is complementary with UWB/laser radar hardware, corrects a positioning result in time, and reduces errors. For example: in practical applications, a certain number of reflectors are suspended in a relatively ideal indoor environment (structured environment, no interference of strong reflective objects, such as windows, other reflective strips, etc., no interference of large metal objects on UWB communication), and the reflectors are composed of a cylindrical carrier with a fixed radius and a high-reflectivity reflective film covered on the cylindrical carrier, as shown in fig. 1. The UWB module is affixed to the reflector and mounted in various suitable locations in the environment. The definition of the appropriate positions is: the height of the reflecting strips is on the same horizontal plane with the laser radar scanner, the angle difference between the reflecting plates and the laser radar scanner is larger than 0.6 degrees, the distance between the reflecting plates and other reflecting sources which possibly cause interference is larger than 0.3m, and the arrangement structure of the reflecting plates in the space cannot form complete symmetry. As shown in fig. 2, 3 and 4: then, the surrounding environment is continuously scanned by using a laser radar, the coordinates of each reflector arranged in the environment are obtained, and the global coordinate (x) of the positioning system at the moment k is calculated by using a triangulation positioning method k ,y k ) And uses the formula:
the displacement of the positioning system in the x and y directions relative to the k-1 moment and the velocity variation are thus determined. The four parameters are combined into the state quantity of the positioning systemWherein: />And->For the coordinate variation of the positioning system in x and y directions at time k with respect to time k-1,/for the positioning system>And->For the speed variation of the positioning system in x and y directions at time k with respect to time k-1. The state equation of the positioning system is set as follows: x is X k ＝A k X k-1 +w k-1 Wherein A is k A state transition matrix for the positioning system; x is X k Is a state vector, w k-1 Is process noise and is assumed to satisfy a mean of 0, variance of +.>Is a gaussian distribution of (c); k represents the time. State transition matrix->Δt is the positioning system sampling time difference. And then, establishing a system measurement equation model. Let the distance between the k moment positioning system and UWB on the ith reflector plate be:
let the distance between the UWB/laser radar system at time k-1 and time k be:
wherein: x is x k Representing the x coordinate, y of the system at time k k Representing the y-coordinate, v, of the system at time k k，i Observing noise of the laser radar/UWB system at the time k; v k，k-1 Is the observed noise of the lidar/UWB system between time k-1 and k. Since equations (6) and (7) are nonlinear functions, binary taylor expansion is performed on the functions, and the second and higher order terms are ignored:
and (3) calculating:
wherein: zeta type toy k，i Observing noise after Taylor expansion at the moment k for the positioning system; zeta type toy k，k-1 Observing noise after Taylor expansion for a positioning system between k-1 and k time; d, d k0，i Representing the distance between the positioning system at time k0 (i.e. the initial time of each calculation process) and UWB on the ith reflector, d k0，k-1 Representing the distance between the k0 moment positioning system and the k moment; x is x k0 Representing the x-coordinate, x of the system at the initial moment k-1 Representing the x coordinate, y of the system at time k-1 k0 Representing the y-coordinate, y of the system at the initial moment k-1 Representing the y-coordinate of the system at time k-1,x-coordinate representing ith UWB, +.>Representing the y-coordinate of the ith UWB. From this, the measurement equation for the positioning system can be written as: y is Y k ＝C k X k +v k
Wherein:
observation matrix C k :
Thus, the mathematical model of the positioning system, namely the state equation and the measurement equation, is built. Then, the state quantity of the system is calculated by using an extended Kalman filter algorithm (EKF), namelyAnd (5) carrying out state prediction and measurement updating to obtain the optimal value of the state quantity of the positioning system at any k time. The method comprises the following specific steps:
the iterative operation process of the EKF algorithm comprises two parts of state prediction and measurement updating: first, a system state quantity X is located k The initial value of (1) is taken asThe mean square error in the initial state is +.>At this time, the system is in the most initial state without any prior value. The EKF formula is as follows:
P k ＝(I-K k C K )P′ k (5)
wherein:an estimated value of the state quantity before correction, namely, a priori estimation of the state quantity; />The state quantity is an estimated value of the corrected state quantity, namely posterior estimation of the state quantity; p'. k The mean square error, namely the prior variance, of the state quantity before correction; p (P) k The mean square error of the corrected state quantity is the post-test square error; q (Q) k-1 Is the process noise variance; r is R k To observe the noise variance. Determination ofAnd->From equations (1) and (4), the a priori estimate of the state quantity at time k=1 can then be determined +.>And a priori variance P' 1 (the two previous calculations, known as state prediction, are predicted by a priori estimates and a priori variances of the state quantities) and then P' 1 To be entered (3) to obtain the kalman gain K at the moment of k=1 1 With K 1 And->Equation (2) allows the determination of +.1 at time k=1>I.e. estimation of the state quantity after correction at time k=1, i.e. a posterior estimation of the state quantity, with K 1 And P' 1 The state quantity mean square error P after the k=1 time correction can be obtained from the equation (5) 1 I.e., the posterior variance, (these two calculations, i.e., the metrology update, updated is the posterior estimate of the state quantity and the posterior variance) the latter operation is a constant iteration of the previous process. It should be noted that, the posterior estimated value of the state quantity obtained by the k moment EKF algorithm through measurement and update is regarded as the prior estimated value of the state quantity in the next moment (k+1 moment) operation in the iterative operation process, and the result of each calculation process is used for updating the observation matrix C in turn k . And obtaining the optimal value of the state quantity of the positioning system at each moment through continuous iterative updating, and further obtaining the positioning result of the positioning system at each moment. From equations (2), (3) and (5), the Kalman gain K of the EKF algorithm is known k Estimated value of corrected state quantity +.>And corrected state quantity mean square error P k And observation matrix C k Correlation, while observing matrix C k Is built based on a "constraint term" such as a distance equation between the lidar and UWB, and thereforeThe mathematical model couples the distance between the laser radar and the UWB as a constraint part to a measurement equation of the whole system, and continuously constrains the result obtained by the final operation in the EKF iteration process, so that the effect of constraint optimization on the system positioning result is achieved, and a specific flow can be shown in fig. 6.
In this embodiment, as shown in fig. 7, a reflector is disposed in a field where the AGV moves, a UWB positioning base station is attached to each reflector, and the UWB antenna and the laser radar form a positioning system of the AGV and are installed on the AGV;
the UWB antenna is used for receiving UWB pulse signals;
the laser radar is used for receiving laser signals reflected by the reflecting plate;
and the calculation module is arranged on the AGV and used for obtaining UWB positioning information and laser positioning information of the AGV according to the received signals.
Specifically, a combined positioning model of the AGV is introduced into a computing module installed on the AGV, and the combined positioning model comprises: the system comprises a state model of an AGV and a measurement model established by using the state model, wherein a reflector is arranged in a place where the AGV moves, a UWB positioning base station is stuck on each reflector, and a positioning system of the AGV is formed by a UWB antenna and a laser radar;
the calculation module is specifically used for inputting the obtained UWB positioning information and laser positioning information into the combined positioning model and updating an observation matrix; and then carrying out iterative operation by using the updated observation matrix through extended Kalman filtering, and carrying out state prediction and measurement updating on the AGV by using the iterative operation result.
In this specification, each embodiment is described in a progressive manner, and identical and similar parts of each embodiment are all referred to each other, and each embodiment mainly describes differences from other embodiments. In particular, for the apparatus embodiments, since they are substantially similar to the method embodiments, the description is relatively simple, and reference is made to the description of the method embodiments for relevant points. The foregoing is merely illustrative of the present invention, and the present invention is not limited thereto, and any changes or substitutions easily contemplated by those skilled in the art within the scope of the present invention should be included in the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims (6)

1. The combined positioning method of the laser radar and the UWB based on the extended Kalman filtering is characterized by comprising the following steps:
s1, establishing a combined positioning model of an AGV, wherein the combined positioning model comprises: the system comprises a state model of an AGV and a measurement model established by using the state model, wherein a reflector is arranged in a place where the AGV moves, a UWB positioning base station is stuck on each reflector, and a positioning system of the AGV is formed by a UWB antenna and a laser radar;
s2, receiving UWB pulse signals through the UWB antenna, receiving laser signals reflected by the reflecting plate through the laser radar, and obtaining UWB positioning information and laser positioning information of the AGV according to the received signals, wherein the step S2 is periodically executed in the running process of the AGV;
s3, inputting the obtained UWB positioning information and laser positioning information into the combined positioning model, and updating an observation matrix;
s4, carrying out iterative operation by using the updated observation matrix through extended Kalman filtering, and carrying out state prediction and measurement updating on the AGV by using the iterative operation result;
in S1, it includes: establishing a state model X of an AGV k ＝A k X k-1 +w k-1 ，A k For locating the state transition matrix of the system, X k Is a state vector, w k-1 For process noise, k represents time;
wherein, and->For the coordinate variation of the positioning system in x and y directions at time k with respect to time k-1,/for the positioning system>And->A speed variation of the positioning system in x and y directions at the time k relative to the time k-1;
the measurement model established by the state model comprises the following components: y is Y k ＝C k X k +v k ，Y K For outputting matrix, v k To measure the noise matrix, C k Is an observation matrix;
wherein,ξ k，i in order to observe noise after Taylor expansion at time k, ζ k，k-1 I represents the number of the reflector and d is the observation noise after Taylor expansion between the k-1 and k moments k，i Represents the distance between the positioning system and the UWB positioning base station on the ith reflector at time k, d k，k-1 Represents the distance between the position of the positioning system at the time k-1 and the position at the time k, d k0，i Representing the distance between the UWB/lidar system and the UWB on the ith reflector, d, at time k0 (i.e., the initial time of each calculation process) k0，k-1 Representing the distance between the UWB/lidar system at time k0 and time k;
x k0 initial time system for representing each calculation processX coordinate, x k-1 Representing the x coordinate, y of the system at time k-1 k0 Representing the y-coordinate, y of the system at the initial time of each calculation process k-1 Represents the y-coordinate of the system at time k-1, < >>X-coordinate representing ith UWB, +.>Representing the y-coordinate of the ith UWB.
2. The method of claim 1, wherein w k-1 Satisfies a mean of 0 and a variance ofIs a gaussian distribution of (c);
the period of step S2 is Δt, then:
3. the method of claim 1, wherein the distance between the positioning system and the UWB positioning base station on the ith reflector at time k
The distance between the position of the positioning system at the time k-1 and the position at the time kx k Representing the x coordinate, y of the system at time k k Representing the y-coordinate, v, of the system at time k k，i V for observation noise at time k k，k-1 Is the observed noise between k-1 and k times.
4. The method of claim 1, wherein the iterative operation by extended kalman filtering comprises:
performing iterative operation through a recurrence formula, wherein the recurrence formula is as follows
And the initial value is taken during iterative operation>Ande denotes solving mathematical expectation (mean) operators, e.g. for X 0 The mean value is E [ X ] 0 ]；X 0 Representing the state quantity of the system at the initial moment; />Representing a state initial value of the system; p (P) 0 Mean square error is the initial state of the system; />An estimated value for the state quantity before correction; />The estimated value of the state quantity after the correction at the moment k; />The estimated value of the state quantity corrected at the moment k-1; k (K) k The Kalman gain of the system at the moment k is represented; p (P) k The mean square error of the state quantity corrected at the moment k; p (P) k-1 The mean square error of the state quantity corrected at the moment k-1; w (W) k Expanding a weight matrix for process noise Taylor; q (Q) k-1 Is the process noise variance; p'. k Mean square error for state quantity before correction; r is R k To observe the noise variance.
5. The method of claim 4 wherein said utilizing the results of the iterative operation to perform status prediction and metrology updates for the AGV comprises:
in each iteration, the prior estimation and the prior variance of the state quantity at the current moment are obtained through the iteration formula, wherein the coordinates of the AGV are used as the state quantity input into the iteration formula;
and acquiring the Kalman gain at the current moment by using the prior variance, and acquiring the posterior estimation by using the prior estimation and the Kalman gain.
6. The laser radar and UWB combined positioning system based on the extended Kalman filtering is characterized in that reflecting plates are arranged in a field where an AGV moves, a UWB positioning base station is adhered to each reflecting plate, and a UWB antenna and the laser radar form a positioning system of the AGV and are arranged on the AGV;
the UWB antenna is used for receiving UWB pulse signals;
the laser radar is used for receiving laser signals reflected by the reflecting plate;
the calculation module is arranged on the AGV and used for obtaining UWB positioning information and laser positioning information of the AGV according to the received signals;
the combined positioning model of AGV has been imported in the calculation module of installing on the AGV, the combined positioning model includes: the system comprises a state model of an AGV and a measurement model established by using the state model, wherein a reflector is arranged in a place where the AGV moves, a UWB positioning base station is stuck on each reflector, and a positioning system of the AGV is formed by a UWB antenna and a laser radar;
the calculation module is specifically used for inputting the obtained UWB positioning information and laser positioning information into the combined positioning model and updating an observation matrix; then, carrying out iterative operation by using the updated observation matrix through extended Kalman filtering, and carrying out state prediction and measurement updating on the AGV by using the iterative operation result;
wherein AG is establishedA combined positioning model of V comprising: establishing a state model X of an AGV k ＝A k X k-1 +w k-1 ，A k For locating the state transition matrix of the system, X k Is a state vector, w k-1 For process noise, k represents time;
wherein, and->For the coordinate variation of the positioning system in x and y directions at time k with respect to time k-1,/for the positioning system>And->A speed variation of the positioning system in x and y directions at the time k relative to the time k-1;
the measurement model established by the state model comprises the following components: y is Y k ＝C k X k +v k ，Y k For outputting matrix, v k To measure the noise matrix, C k Is an observation matrix;
wherein,ξ k，i in order to observe noise after Taylor expansion at time k, ζ k，k-1 I represents the number of the reflector and d is the observation noise after Taylor expansion between the k-1 and k moments k，i Represents the distance between the positioning system and the UWB positioning base station on the ith reflector at time k, d k，k-1 Representing the distance between the position of the positioning system at time k-1 and the position at time kSeparation, d k0，i Representing the distance between the UWB/lidar system and the UWB on the ith reflector, d, at time k0 (i.e., the initial time of each calculation process) k0，k-1 Representing the distance between the UWB/lidar system at time k0 and time k;
x k0 x-coordinate, x, representing the system at the initial moment of each calculation process k-1 Representing the x coordinate, y of the system at time k-1 k0 Representing the y-coordinate, y of the system at the initial time of each calculation process k-1 Represents the y-coordinate of the system at time k-1, < >>Representing the x-coordinate of the ith UWB,representing the y-coordinate of the ith UWB.
CN202210931857.XA 2022-08-04 2022-08-04 Laser radar and UWB combined positioning method and system based on extended Kalman filtering Active CN115421153B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210931857.XA CN115421153B (en) 2022-08-04 2022-08-04 Laser radar and UWB combined positioning method and system based on extended Kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210931857.XA CN115421153B (en) 2022-08-04 2022-08-04 Laser radar and UWB combined positioning method and system based on extended Kalman filtering

Publications (2)

Publication Number Publication Date
CN115421153A CN115421153A (en) 2022-12-02
CN115421153B true CN115421153B (en) 2024-02-13

Family

ID=84195793

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210931857.XA Active CN115421153B (en) 2022-08-04 2022-08-04 Laser radar and UWB combined positioning method and system based on extended Kalman filtering

Country Status (1)

Country Link
CN (1) CN115421153B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116125380B (en) * 2023-04-19 2023-06-30 齐鲁工业大学(山东省科学院) Mobile scene super-resolution positioning method based on Kalman filter

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN209230639U (en) * 2018-11-07 2019-08-09 成都方德尔科技有限公司 A kind of coordinate location device based on laser navigation
CN113721278A (en) * 2021-08-25 2021-11-30 上海交通大学 Positioning method and system of derusting wall-climbing robot for splicing seams of hull outer plates
CN114047519A (en) * 2021-09-26 2022-02-15 苏州摩擎科技有限公司 AGV positioning system and method based on ultra-wideband and laser SLAM (synchronous map positioning and navigation) composite navigation technology
CN114739400A (en) * 2022-04-11 2022-07-12 中国科学院合肥物质科学研究院 Indoor positioning method based on UWB and IMU information fusion

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11126198B2 (en) * 2018-12-30 2021-09-21 Ubtech Robotics Corp Robot movement control method, apparatus and robot using the same

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN209230639U (en) * 2018-11-07 2019-08-09 成都方德尔科技有限公司 A kind of coordinate location device based on laser navigation
CN113721278A (en) * 2021-08-25 2021-11-30 上海交通大学 Positioning method and system of derusting wall-climbing robot for splicing seams of hull outer plates
CN114047519A (en) * 2021-09-26 2022-02-15 苏州摩擎科技有限公司 AGV positioning system and method based on ultra-wideband and laser SLAM (synchronous map positioning and navigation) composite navigation technology
CN114739400A (en) * 2022-04-11 2022-07-12 中国科学院合肥物质科学研究院 Indoor positioning method based on UWB and IMU information fusion

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于多传感器融合技术的室内移动机器人定位研究;张书亮 等;传感器与微系统;第40卷(第8期);正文第53-56页 *
超宽带与激光雷达组合的室内定位方法;杨东辉 等;测绘科学;第44卷(第6期);正文第72-77页 *

Also Published As

Publication number Publication date
CN115421153A (en) 2022-12-02

Similar Documents

Publication Publication Date Title
He et al. An integrated GNSS/LiDAR-SLAM pose estimation framework for large-scale map building in partially GNSS-denied environments
US10175348B2 (en) Use of range-rate measurements in a fusion tracking system via projections
CN104035439B (en) BAYESIAN NETWORK TO TRACK OBJECTS USING SCAN POINTS USING MULTIPLE LiDAR SENSORS
CN113074739B (en) UWB/INS fusion positioning method based on dynamic robust volume Kalman
Kellner et al. Instantaneous lateral velocity estimation of a vehicle using Doppler radar
CN112904358B (en) Laser positioning method based on geometric information
Thormann et al. Extended target tracking using Gaussian processes with high-resolution automotive radar
Park et al. Radar localization and mapping for indoor disaster environments via multi-modal registration to prior LiDAR map
CN109884586A (en) Unmanned plane localization method, device, system and storage medium based on ultra-wide band
CN112731337B (en) Map construction method, device and equipment
JP7155284B2 (en) Measurement accuracy calculation device, self-position estimation device, control method, program and storage medium
CN108332752B (en) Indoor robot positioning method and device
CN112198503A (en) Target track prediction optimization method and device and radar system
CN112505671B (en) Millimeter wave radar target positioning method and device under GNSS signal missing environment
CN115421153B (en) Laser radar and UWB combined positioning method and system based on extended Kalman filtering
CN114623823B (en) UWB (ultra wide band) multi-mode positioning system, method and device integrating odometer
CN115950418A (en) Multi-sensor fusion positioning method
CN116772831A (en) Mobile robot navigation and positioning system and method based on multi-sensor fusion
Lee et al. Experimental results and posterior cramér–rao bound analysis of EKF-based radar SLAM with odometer bias compensation
CN118310531A (en) Cross-scene positioning method and system for robot with coarse-to-fine point cloud registration
Thomas et al. Comparison of nearest neighbor and probabilistic data association filters for target tracking in cluttered environment
Lupfer et al. Increasing FastSLAM accuracy for radar data by integrating the Doppler information
Lategahn et al. Global localization of automated guided vehicles in wireless networks
Kwon et al. Radar velocity measurements aided navigation system for UAVs
CN113891251B (en) Inertial navigation-WiFi signal fusion positioning algorithm based on AGV

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