CN115334448A - Accurate dynamic positioning method of unmanned self-following device based on Bluetooth and inertial sensor - Google Patents
Accurate dynamic positioning method of unmanned self-following device based on Bluetooth and inertial sensor Download PDFInfo
- Publication number
- CN115334448A CN115334448A CN202210974622.9A CN202210974622A CN115334448A CN 115334448 A CN115334448 A CN 115334448A CN 202210974622 A CN202210974622 A CN 202210974622A CN 115334448 A CN115334448 A CN 115334448A
- Authority
- CN
- China
- Prior art keywords
- bluetooth
- following device
- inertial sensor
- positioning
- coordinate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 27
- 238000001914 filtration Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 6
- 238000009795 derivation Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 239000000463 material Substances 0.000 claims description 3
- LFQSCWFLJHTTHZ-UHFFFAOYSA-N Ethanol Chemical compound CCO LFQSCWFLJHTTHZ-UHFFFAOYSA-N 0.000 claims description 2
- 238000005259 measurement Methods 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 7
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- 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
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/029—Location-based management or tracking services
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/80—Services using short range communication, e.g. near-field communication [NFC], radio-frequency identification [RFID] or low energy communication
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
A method for accurately and dynamically positioning an unmanned self-following device based on Bluetooth and an inertial sensor comprises the following positioning steps: 1) The four Bluetooth base stations respectively obtain the distances between the four Bluetooth base stations and the Bluetooth beacon, and the four-point positioning algorithm is adopted to resolve observation coordinate data; calculating prior coordinate data by using an inertial sensor; 2) Fusing the prior coordinate data and the observation coordinate data through a Kalman filtering algorithm to obtain posterior coordinate data, and accurately positioning the unmanned self-following device; 3) When the disconnection phenomenon occurs, the position coordinate of the bluetooth beacon positioned at the last time before disconnection is taken as a target point, inertial navigation is carried out by using the inertial sensor, the unmanned self-following device is controlled to move to the target point to wait for reconnection, the bluetooth signal is reconnected, or the bluetooth signal is reconnected in the process of moving to the target point, and then accurate positioning is carried out by continuously using the signals of the bluetooth and the inertial sensor.
Description
Technical Field
The invention belongs to the field of unmanned self-following device control, and particularly relates to an accurate dynamic positioning method of an unmanned self-following device based on Bluetooth and an inertial sensor.
Background
In recent years, with the development of computer technology and wireless communication technology, intelligent devices are more and more frequently appeared in the life of people, and particularly, the devices are automatically followed by no people. Use unmanned aerial vehicle as an example, unmanned aerial vehicle has all obtained extensive application in military and civilian field, and wherein one of unmanned aerial vehicle's core technology is regarded as to the automatic technology of following, has obtained the extensive research of academic and industrial circles, and the realization of automatic technology of following mainly has two kinds of technical scheme: (1) Based on automatic following of GPS positioning, the technical scheme needs to respectively install GPS modules in an unmanned aerial vehicle and mobile equipment (such as a remote controller), the relative positions of the unmanned aerial vehicle and a shot person are determined through the precise positioning of the GPS, a flight control system controls the unmanned aerial vehicle to realize an automatic following function according to the relative positions of the unmanned aerial vehicle and the shot person, and the scheme must ensure the accuracy of GPS signals and cannot be used in mountainous areas or indoor areas with weak signals; (2) Based on visual following, the technology requires that the following target has high identification degree relative to the surrounding environment, and meanwhile, the following target needs to be always kept in the capture range of a camera, and the target cannot be found, so that the visual following is forced to stop temporarily.
Disclosure of Invention
The invention aims to provide a precise dynamic positioning method of an unmanned self-following device based on Bluetooth and an inertial sensor, aiming at the defects of the prior art. According to the method, the inertial sensor and the Bluetooth RSSI ranging and positioning are utilized to perform data fusion, so that the accurate dynamic positioning of the automatic following device relative to the mobile terminal is realized, the defect that GPS self-following cannot work in mountainous areas or indoors with poor signals is overcome, the requirement on environment identification degree is not high, and meanwhile, the problems of signal instability and large error are solved after data fusion processing based on Kalman filtering is performed on data of the Bluetooth signal and the inertial sensor.
The technical scheme of the invention is as follows:
an accurate dynamic positioning method of an unmanned self-following device based on Bluetooth and an inertial sensor,
the system comprises a dynamic positioning system consisting of an unmanned self-following device provided with an inertial sensor and four Bluetooth base stations and a handheld Bluetooth beacon, wherein the positioning system is positioned according to the following steps:
1) The four Bluetooth base stations respectively obtain the distances between the four Bluetooth base stations and the Bluetooth beacons through an RSSI ranging method, and the position coordinates of the Bluetooth beacons are calculated by adopting a four-point positioning algorithm to serve as observation coordinate data; meanwhile, the position coordinates of the Bluetooth beacon are calculated out as prior coordinate data by utilizing the speed of the unmanned self-following device calculated by the inertial sensor;
2) Fusing the prior coordinate data and the observation coordinate data through a Kalman filtering algorithm to obtain posterior coordinate data, and accurately positioning the unmanned self-following device;
3) When the Bluetooth base station and the Bluetooth beacon are disconnected, the position coordinate of the Bluetooth beacon located at the last time before disconnection is taken as a target point, inertial navigation is carried out by using an inertial sensor, the unmanned self-following device is controlled to move to the target point and stop to wait for reconnection of a Bluetooth signal, and once the Bluetooth signal is reconnected, accurate positioning is carried out by continuously using signals of the Bluetooth and the inertial sensor; if the unmanned self-following device reconnects the Bluetooth signal in the process of moving to the target point, the signals of the Bluetooth and the inertial sensor are continuously utilized for accurate positioning.
Further, the bluetooth beacon is integrated in a remote controller, or bluetooth of a smart phone is used as the bluetooth beacon.
Further, unmanned following device is unmanned aerial vehicle or material transport vechicle.
Further, the bluetooth beacon is a bluetooth 5.0 beacon, and the bluetooth base station is a bluetooth 5.0 base station or a signal transmitter.
Further, the RSSI ranging method in step 1) is to construct a logarithmic signal strength-distance attenuation model by using the distance between the receiving end and the transmitting end, and convert the signal strength data measured by each bluetooth base station into the relative distance between the bluetooth base station and the bluetooth beacon, where the relationship between the signal strengths of the bluetooth signals at the transmitting end and the receiving end is as follows:
L p (dB)＝L 0 (dB)+10αlg d+ω(dB)
wherein L is 0 Represents the signal energy loss at a distance of 1 meter; l is a radical of an alcohol p Represents the signal energy loss at distance d; α represents a path loss exponent; ω represents the error; d represents a straight-line distance between the transmitting end and the receiving end.
Further, in step 1), the specific process of the four-point positioning algorithm for resolving the observation position coordinates of the bluetooth beacon is as follows:
(x-x 1 ) 2 +(y-y 1 ) 2 +(z-z 1 ) 2 ＝R 1 2
(x-x 2 ) 2 +(y-y 2 ) 2 +(z-z 2 ) 2 ＝R 2 2
(x-x 3 ) 2 +(y-y 3 ) 2 +(z-z 3 ) 2 ＝R 3 2
(x-x 4 ) 2 +(y-y 4 ) 2 +(z-z 4 ) 2 ＝R 4 2
wherein (x) 1 ，y 1 ，z 1 )，(x 2 ，y 2 ，z 2 )，(x 3 ，y 3 ，z 3 )，(x 4 ，y 4 ，z 4 ) Coordinates of four bluetooth base stations, R, respectively 1 ，R 2 ，R 3 ，R 4 The relative distances between the four Bluetooth base stations and the Bluetooth beacon are measured respectively, and then (x, y, z) is the position coordinate of the Bluetooth beacon;
estimating by adopting a least square method, and sorting the above formula to obtain a matrix form of equation solution:
AX＝B
in the formula, A represents a coefficient matrix, X represents a three-dimensional coordinate of the Bluetooth beacon in a coordinate system of the unmanned self-following device, and B represents a three-dimensional vector after the linear equation set is arranged;
the error vector is: epsilon = AX-B, then there is,
F(x)＝||ε|| 2 ＝(AX-B) T (AX-B)
wherein F (x) represents the square of the two-norm of the error vector;
if the error is to be minimized, F (X) is minimized, and X is differentiated by the above formula to make its derivative 0, and the calculation formula is as follows:
solved and obtained X = (A) T A) -1 A T B, the error F (X) is minimized at this time, and the final X coordinate can be calculated.
Further, step 1) adopts a combined dynamic positioning algorithm of a bluetooth signal and an inertial sensor, and specifically comprises the following steps:
(1) Keeping the z-axis height of the unmanned self-following device unchanged, and inputting the initial value (x) of the position coordinate of the Bluetooth beacon 0 ，y 0 ) The initial value is the Bluetooth beacon coordinate position obtained through the Bluetooth RBSI value for the first time;
(2) The inertial sensor respectively measures the acceleration a of the unmanned self-following device along the x axis x And acceleration a along the y-axis y By the pair of accelerations a x And a y Obtaining the speed v of the unmanned self-following device along the x axis by integration x And velocity v along the y-axis y ；
(3) Angular velocity ω along the z-axis output by inertial sensors z Multiplying the sampling time dt to obtain the heading angle change d theta of two adjacent sampling of the inertial sensor, wherein the formula is as follows:
dθ＝dt·w z ；
(4) Calculating prior coordinate data (x (k + 1), y (k + 1)) at the next time according to the inertial sensor data obtained in the steps (2) and (3) and the posterior coordinate data (x (k), y (k)) at the previous time, wherein the calculation formula is as follows:
writing the above equation in matrix form:
(5) The RSSI ranging of the Bluetooth is combined with a four-point positioning algorithm to calculate the position coordinates of the Bluetooth beacon as observation data, and the observation coordinate data equation is as follows:
in the formula, (x '(k + 1), y' (k + 1)) is observation coordinate data at the time of k + 1.
Further, the kalman filtering algorithm in step 2) is:
(1) Initializing parameters:
a priori state quantity
Posterior state quantity
Input device
Process error covariance matrix:
measuring a noise covariance matrix:
initial state quantity
In the formula, x - X-axis prior coordinate value, y, of bluetooth beacon representing unmanned self-following device - Y-axis prior coordinate value, x, of bluetooth beacon representing unmanned self-following device + X-axis posterior coordinate value, y, of bluetooth beacon representing unmanned self-following device + The y-axis posterior coordinate value of the Bluetooth beacon of the unmanned self-following device is represented, and X (0) represents initial observation coordinate data of the Bluetooth beacon of the unmanned self-following device;
(2) Iteratively updating the optimal posterior coordinate data:
X - (K+1)＝AX + (k)+Bu(k)
P - (K+1)＝AP + A T +Q
S＝HP - (K+1)H T +R
K＝P - (K+1)H T S -1
X + (k+1)＝X - (k+1)+K(Z(k+1)-HX - (k+1))
P + (k+1)＝(I-KH)P - (k+1)
the posterior state quantity X after the k +1 iteration + (K + 1) as the final positioning position coordinate to control the unmanned aerial vehicle to realize the automatic following function;
in the formula, data with superscripts are prior data, data with superscripts + are posterior data, P is a state covariance matrix which is an iteratively updated quantity, an initial value of the quantity is a second-order unit matrix, parameters of Q and R matrixes are set according to actual engineering debugging conditions, S represents an intermediate quantity for formula derivation, K represents a Kalman gain obtained through calculation, and Z (K + 1) represents observation coordinate data of the Bluetooth beacon.
The beneficial effects of adopting the above technical scheme are as follows: the method has the advantages of high positioning precision, stable positioning performance, low implementation difficulty, low cost of required equipment and easy deployment, and is suitable for the technology of dynamically and accurately positioning the moving target by the automatic following device. The invention effectively reduces the positioning error caused by the environmental influence in the transmission process of the Bluetooth signal, and provides a solution after the signal is lost: and the automatic following device is controlled to search for the Bluetooth signal and request for a reconnection signal only by means of inertial navigation. The invention provides a technical scheme with low cost, good effect and low difficulty for realizing the self-following function of the automatic following device.
Drawings
FIG. 1 is a schematic diagram of the installation positions of a Bluetooth 5.0 base station and an inertial sensor;
FIG. 2 is a schematic representation of the calculation of a priori coordinate data of a beacon using inertial sensors;
fig. 3 is a flow chart of unmanned aerial vehicle self-following accurate dynamic positioning.
Detailed Description
An accurate dynamic positioning method of an unmanned self-following device based on Bluetooth and an inertial sensor,
comprises a dynamic positioning system consisting of an unmanned self-following device provided with an inertial sensor and four Bluetooth base stations and a handheld Bluetooth beacon,
wherein, the bluetooth basic station is bluetooth 5.0 basic station or signal transmitter, and unmanned self-following device is unmanned aerial vehicle or unmanned materials transport vechicle, and unmanned aerial vehicle is used as the example to this example, and hand-held type bluetooth beacon adopts bluetooth 5.0 beacon, can integrate in unmanned aerial vehicle's remote controller, or adopts the smart mobile phone bluetooth as bluetooth beacon.
The positioning system comprises the following steps:
1) The Bluetooth signal strength of the mobile phone or the handheld Bluetooth beacon is detected through the four Bluetooth base stations.
2) Carrying out average value filtering processing on the Bluetooth signal intensity;
3) The four Bluetooth base stations respectively obtain the distances between the four Bluetooth base stations and the Bluetooth beacons through an RSSI ranging method, and then the position coordinates of the Bluetooth beacons are calculated by adopting a four-point positioning algorithm to serve as observation coordinate data, wherein the specific algorithm is as follows:
3-1) the RSSI ranging method is that a logarithmic signal strength-distance attenuation model is constructed by utilizing the distance between a receiving end and a transmitting end, the signal strength data measured by each Bluetooth base station is converted into the relative distance between the Bluetooth base station and a Bluetooth beacon, and the signal strength relation of the Bluetooth signals at the transmitting end and the receiving end is as follows:
L p (dB)＝L 0 (dB)+10αlg d+ω(dB)
wherein L is 0 Represents the signal energy loss at a distance of 1 meter; l is p Represents the signal energy loss at distance d; a represents a path loss exponent; ω represents the error, usually seen as zero mean gaussian noise, whose variance is dependent on the environment; d represents the linear distance between the transmitting end and the receiving end.
3-2) the specific process of resolving the observation position coordinates of the Bluetooth beacon by the four-point positioning algorithm is as follows:
(x-x 1 ) 2 +(y-y 1 ) 2 +(z-z 1 ) 2 ＝R 1 2
(x-x 2 ) 2 +(y-y 2 ) 2 +(z-z 2 ) 2 ＝R 2 2
(x-x 3 ) 2 +(y-y 3 ) 2 +(z-z 3 ) 2 ＝R 3 2
(x-x 4 ) 2 +(y-y 4 ) 2 +(z-z 4 ) 2 ＝R 4 2
wherein (x) 1 ，y 1 ，z 1 )，(x 2 ，y 2 ，z 2 )，(x 3 ，y 3 ，z 3 )，(x 4 ，y 4 ，z 4 ) Respectively representing the coordinates of four Bluetooth base stations, R 1 ，R 2 ，R 3 ，R 4 The relative distances between the four Bluetooth base stations and the Bluetooth beacon are measured respectively, and then (x, y, z) is the position coordinate of the Bluetooth beacon;
under the condition that the measured distance between the Bluetooth base station and the Bluetooth beacon has no error, the Bluetooth base station is used as a circle center, the measured distance is a positioning sphere with a radius, the positioning sphere is intersected at one point, the Bluetooth beacon is located at the intersection point, and the Bluetooth signal is usually subjected to multipath and non-line-of-sight errors in the process of propagation measurement, wherein the errors are usually positive values, so that a plurality of positioning circles are intersected pairwise, and a plurality of intersection points are formed.
Therefore, the least square method is adopted for estimation, and the above formula is arranged to obtain a matrix form of equation solution:
AX＝B
in the formula: a represents a coefficient matrix, X represents a three-dimensional coordinate of the Bluetooth beacon under the coordinate system of the unmanned self-following device, and B represents a three-dimensional vector after the linear equation set is arranged;
the error vector is: ε = AX-B, then there are:
F(x)＝||ε|| 2 ＝(AX-B) T (AX-B)
wherein F (x) represents the square of the two-norm of the error vector;
if the error is to be minimized, F (X) is minimized, and X is differentiated by the above formula to make its derivative 0, and the calculation formula is as follows:
solved and obtained X = (A) T A) -1 A T And B, minimizing the error F (X) at the moment, so that a final X coordinate can be calculated, namely the position coordinate of the Bluetooth beacon is obtained as observation coordinate data.
4) The method is characterized in that the position coordinate of the Bluetooth beacon is calculated by the speed of the unmanned self-following device calculated by the inertial sensor and serves as prior coordinate data, and the specific algorithm is as follows:
because the unmanned aerial vehicle follows at a fixed height, the z value is kept unchanged, and the unmanned aerial vehicle is controlled at a certain flight height to be unchanged when a flight control algorithm is carried out, so that a three-dimensional space coordinate can be converted into a two-dimensional coordinate to discuss in the moving process of following the Bluetooth beacon, namely only the values of an x axis and a y axis are concerned;
4-1) keeping the z-axis height of the unmanned self-following device unchanged, and inputting an initial value (x) of the position coordinate of the Bluetooth beacon 0 ，y 0 ) The initial value is the Bluetooth beacon coordinate position obtained through the Bluetooth RSSI value for the first time;
4-2) the inertial sensor respectively measures the acceleration a of the unmanned self-following device along the x axis x And acceleration a along the y-axis y By the pair of accelerations a x And a y Obtaining the speed v of the unmanned self-following device along the x axis by integration x And velocity v along the y-axis y ；
4-3) angular velocity ω along the z-axis output by the inertial sensor z Multiplying by sampling time dt to obtain course angle change d theta of two adjacent samplings of the inertial sensor, wherein the iteration period of the algorithm is 20ms, so that the Bluetooth beacon is considered to be kept delicate in the sequential iteration process, and the algorithm formula is as follows:
dθ＝dt·w z ；
4-4) calculating the prior coordinate data (x (k + 1), y (k + 1)) of the next time according to the inertial sensor data obtained in the steps (2) and (3) and the posterior coordinate data (x (k), y (k)) of the previous time, and calculating the formula as follows:
writing the above equation in matrix form:
4-5) calculating the position coordinates of the Bluetooth beacon as observation data by the RSSI ranging of the Bluetooth and combining a four-point positioning algorithm, wherein the equation of the observation coordinate data is as follows:
in the formula, (x '(k + 1), y' (k + 1)) is observed coordinate data at the time of k +1, and a specific algorithm for observing coordinate data is shown in step 3).
5) Fusing the prior coordinate data and the observation coordinate data through a Kalman filtering algorithm to obtain posterior coordinate data, and accurately positioning the unmanned self-following device;
the Kalman filtering algorithm is as follows:
5-1) initialization parameters:
a priori state quantity
Posterior state quantity
Input the method
Process error covariance matrix:
measuring a noise covariance matrix:
initial state quantity
In the formula, x - X-axis prior coordinate value, y, representing unmanned self-following device bluetooth beacon - Y-axis prior coordinate value, x, of bluetooth beacon representing unmanned self-following device + Express that unmanned from the posterior coordinate value of x axle of following device bluetooth beacon, y + The y-axis posterior coordinate value of the Bluetooth beacon of the unmanned self-following device is represented, and X (0) represents initial observation coordinate data of the Bluetooth beacon of the unmanned self-following device;
5-2) iteratively updating the optimal posterior coordinate data:
X - (K+1)＝AX + (k)+Bu(k)
P - (K+1)＝AP + A T +Q
S＝HP - (K+1)H T +R
K＝P - (K+1)H T S -1
X + (k+1)＝X - (k+1)+K(Z(k+1)-HX - (k+1))
P + (k+1)＝(I-KH)P - (k+1)
the posterior state quantity X after the k +1 iteration + (K + 1) as the final positioning position coordinate to control the unmanned aerial vehicle to realize the automatic following function;
in the formula, data with superscripts are prior data, data with superscripts + are posterior data, P is a state covariance matrix which is an iteratively updated quantity, an initial value of the quantity is a second-order unit matrix, parameters of Q and R matrixes are set according to actual engineering debugging conditions, S represents an intermediate quantity for formula derivation, K represents a Kalman gain obtained through calculation, and Z (K + 1) represents observation coordinate data of the Bluetooth beacon.
6) When the Bluetooth base station and the Bluetooth beacon are disconnected, the position coordinate of the Bluetooth beacon positioned at the last time before disconnection is taken as a target point, an inertial sensor is used for inertial navigation, the unmanned self-following device is controlled to move to the target point and stop to wait for reconnection of a Bluetooth signal, once the Bluetooth signal is reconnected, the signals of the Bluetooth and the inertial sensor are continuously fused through a Kalman filtering algorithm to obtain posterior coordinate data, and the unmanned aerial vehicle is accurately positioned through the posterior coordinate data; if the unmanned self-following device is reconnected with the Bluetooth signal in the process of moving to the target point, the posterior coordinate data is obtained by fusing the signals of the Bluetooth and the inertial sensor through a Kalman filtering algorithm, and the posterior coordinate data is used for accurate positioning.
7) And after reconnection, circulating the steps to perform accurate dynamic positioning on the unmanned aerial vehicle.
Claims (8)
1. An accurate dynamic positioning method of an unmanned self-following device based on Bluetooth and an inertial sensor is characterized in that,
the system comprises a dynamic positioning system consisting of an unmanned self-following device provided with an inertial sensor and four Bluetooth base stations and a handheld Bluetooth beacon, wherein the positioning system is positioned according to the following steps:
1) The four Bluetooth base stations respectively obtain the distances between the four Bluetooth base stations and the Bluetooth beacons through an RSSI ranging method, and the position coordinates of the Bluetooth beacons are calculated by adopting a four-point positioning algorithm to serve as observation coordinate data; meanwhile, the position coordinates of the Bluetooth beacon are calculated out as prior coordinate data by utilizing the speed of the unmanned self-following device calculated by the inertial sensor;
2) Fusing the prior coordinate data and the observation coordinate data through a Kalman filtering algorithm to obtain posterior coordinate data, and accurately positioning the unmanned self-following device;
3) When the Bluetooth base station and the Bluetooth beacon are disconnected, the position coordinate of the Bluetooth beacon located at the last time before disconnection is taken as a target point, inertial navigation is carried out by using an inertial sensor, the unmanned self-following device is controlled to move to the target point and stop to wait for reconnection of a Bluetooth signal, and once the Bluetooth signal is reconnected, accurate positioning is carried out by continuously using signals of the Bluetooth and the inertial sensor; if the unmanned self-following device is reconnected with the Bluetooth signal in the process of moving to the target point, the signals of the Bluetooth and the inertial sensor are continuously utilized for accurate positioning.
2. The positioning method according to claim 1, characterized in that: the Bluetooth beacon is integrated in the remote controller, or Bluetooth of the smart phone is used as the Bluetooth beacon.
3. The positioning method according to claim 1, characterized in that: unmanned following device is unmanned aerial vehicle or material transport vechicle.
4. The positioning method according to claim 1, characterized in that: the Bluetooth beacon is a Bluetooth 5.0 beacon, and the Bluetooth base station is a Bluetooth 5.0 base station or a signal transmitter.
5. The method as claimed in claim 1, wherein the RSSI ranging method in step 1) is implemented by constructing a logarithmic signal strength-distance attenuation model by using the distance between the receiving end and the transmitting end, and converting the signal strength data measured by each bluetooth base station into the relative distance between the bluetooth base station and the bluetooth beacon, wherein the relationship between the signal strengths of the bluetooth signal at the transmitting end and the receiving end is as follows:
L p (dB)＝L 0 (dB)+10αlg d+ω(dB)
wherein L is 0 Represents the signal energy loss at a distance of 1 meter; l is a radical of an alcohol p Represents the signal energy loss at distance d; a represents a path loss exponent; ω represents the error; d represents the linear distance between the transmitting end and the receiving end.
6. The positioning method according to claim l, wherein in step 1), the specific process of the four-point positioning algorithm to solve the coordinates of the observed position of the bluetooth beacon is as follows:
(x-x 1 ) 2 +(y-y 1 ) 2 +(z-z 1 ) 2 ＝R 1 2
(x-x 2 ) 2 +(y-y 2 ) 2 +(z-z 2 ) 2 ＝R 2 2
(x-x 3 ) 2 +(y-y 3 ) 2 +(z-z 3 ) 2 ＝R 3 2
(x-x 4 ) 2 +(y-y 4 ) 2 +(z-z 4 ) 2 ＝R 4 2
wherein (x) 1 ，y 1 ，z 1 )，(x 2 ，y 2 ，z 2 )，(x 3 ，y 3 ，z 3 )，(x 4 ，y 4 ，z 4 ) Coordinates, R, of four Bluetooth base stations, respectively 1 ，R 2 ，R 3 ，R 4 The relative distances between the four Bluetooth base stations and the Bluetooth beacon are measured respectively, and then (x, y, z) is the position coordinate of the Bluetooth beacon;
estimating by adopting a least square method, and sorting the above formula to obtain a matrix form of equation solution:
AX＝B
in the formula: a represents a coefficient matrix, X represents a three-dimensional coordinate of the Bluetooth beacon under the coordinate system of the unmanned self-following device, and B represents a three-dimensional vector after the linear equation set is arranged;
the error vector is: ε = AX-B, then:
F(x)＝||ε|| 2 ＝(AX-B) T (AX-B)
wherein F (x) represents the square of the two-norm of the error vector;
if the error is minimized, i.e. F (X) is minimized, the derivative of X is derived to 0 by the above formula, which is calculated as follows:
solving the obtained X = (A) T A) -1 A T B, this minimizes the error F (X), from which the final X coordinate can be calculated.
7. The positioning method according to claim 1, wherein step 1) employs a combined dynamic positioning algorithm of bluetooth signals and inertial sensors, specifically:
(1) Keeping the z-axis height of the unmanned self-following device unchanged, and inputting an initial value (x) of the position coordinates of the Bluetooth beacon 0 ，y 0 ) The initial value is the Bluetooth beacon coordinate position obtained through the Bluetooth RSSI value for the first time;
(2) The inertial sensor respectively measures the acceleration a of the unmanned self-following device along the x axis x And acceleration a along the y-axis y By the pair of accelerations a x And a y Obtaining the speed v of the unmanned self-following device along the x axis by integration x And velocity v along the y-axis y ；
(3) Angular velocity ω along z-axis output by inertial sensor z Multiplying the sampling time dt to obtain the heading angle change d theta of two adjacent sampling of the inertial sensor, wherein the formula is as follows:
dθ＝dt·w z ；
(4) Calculating the prior coordinate data (x (k + 1), y (k + 1)) at the next moment according to the inertial sensor data obtained in the steps (2) and (3) and the posterior coordinate data (x (k), y (k)) at the previous moment, wherein the calculation formula is as follows:
writing the above equation in matrix form:
(5) The RSSI range measurement through the bluetooth combines the four-point positioning algorithm to calculate the position coordinates of the bluetooth beacon as observation data, and the equation of the observation coordinate data is as follows:
in the formula, (x '(k + 1), y' (k + 1)) is observed coordinate data at the time of k + 1.
8. The positioning method according to claim i, wherein the kalman filtering algorithm of step 2) is:
(1) Initializing parameters:
a priori state quantity
Posterior quantity of state
Input device
Process error covariance matrix:
measuring a noise covariance matrix:
initial state quantity
In the formula, x - X-axis prior coordinate value, y, of bluetooth beacon representing unmanned self-following device - Y-axis prior coordinate value, x, of bluetooth beacon representing unmanned self-following device + X-axis posterior coordinate value, y, of bluetooth beacon representing unmanned self-following device + The y-axis posterior coordinate value of the Bluetooth beacon of the unmanned self-following device is represented, and X (0) represents initial observation coordinate data of the Bluetooth beacon of the unmanned self-following device;
(2) Iteratively updating the optimal posterior coordinate data:
X - (K+1)＝AX + (k)+Bu(k)
P - (K+1)＝AP + A T +Q
S＝HP - (K+1)H T +R
K＝P - (K+1)H T S -1
X + (k+1)＝X - (k+1)+K(Z(k+1)-HX - (k+1))
P + (k+1)＝(I-KH)P - (k+1)
the posterior state quantity X after the (k + 1) th iteration is processed + (K + 1) as the final positioning position coordinate to control the unmanned aerial vehicle to realize the automatic following function;
in the formula, data with superscripts is prior data, data with superscripts + is posterior data, P is a state covariance matrix which is an iteratively updated quantity, an initial value of the iteratively updated quantity is set to be a second-order unit matrix, parameters of Q and R matrixes are set according to actual engineering debugging conditions, S represents an intermediate quantity used for formula derivation, K represents Kalman gain obtained through calculation, and Z (K + 1) represents observation coordinate data of the Bluetooth beacon.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210974622.9A CN115334448B (en) | 2022-08-15 | 2022-08-15 | Accurate dynamic positioning method of unmanned self-following device based on Bluetooth and inertial sensor |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210974622.9A CN115334448B (en) | 2022-08-15 | 2022-08-15 | Accurate dynamic positioning method of unmanned self-following device based on Bluetooth and inertial sensor |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115334448A true CN115334448A (en) | 2022-11-11 |
| CN115334448B CN115334448B (en) | 2024-03-15 |
Family
ID=83924073
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210974622.9A Active CN115334448B (en) | 2022-08-15 | 2022-08-15 | Accurate dynamic positioning method of unmanned self-following device based on Bluetooth and inertial sensor |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115334448B (en) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107734457A (en) * | 2017-09-29 | 2018-02-23 | 桂林电子科技大学 | Wisdom parking ground navigation system and method |
| CN109612466A (en) * | 2018-11-30 | 2019-04-12 | 北斗天地股份有限公司山东分公司 | A kind of automobile-used multi-sensor combined navigation method and system in underground |
| CN109782227A (en) * | 2019-02-20 | 2019-05-21 | 核芯互联科技(青岛)有限公司 | A kind of indoor orientation method based on Bluetooth signal RSSI |
| WO2019239365A1 (en) * | 2018-06-13 | 2019-12-19 | Purohit Ankit | System and method for position and orientation tracking of multiple mobile devices |
| CN111024075A (en) * | 2019-12-26 | 2020-04-17 | 北京航天控制仪器研究所 | Pedestrian navigation error correction filtering method combining Bluetooth beacon and map |
| CN111681452A (en) * | 2020-01-19 | 2020-09-18 | 重庆大学 | Unmanned vehicle dynamic lane change track planning method based on Frenet coordinate system |
| CN111726757A (en) * | 2020-05-25 | 2020-09-29 | 南京理工大学 | Indoor parking lot positioning and navigation method based on Bluetooth |
| WO2021139590A1 (en) * | 2020-01-06 | 2021-07-15 | 三个机器人公司 | Indoor localization and navigation apparatus based on bluetooth and slam, and method therefor |
| CN114509069A (en) * | 2022-01-25 | 2022-05-17 | 南昌大学 | Indoor navigation positioning system based on Bluetooth AOA and IMU fusion |
| CN114894185A (en) * | 2022-05-09 | 2022-08-12 | 南昌大学 | Carrier attitude zero-speed correction system based on fusion of Bluetooth AOA and IMU |
| CN115103437A (en) * | 2022-04-27 | 2022-09-23 | 电子科技大学 | Bluetooth and inertial measurement unit tightly-coupled indoor positioning method |
-
2022
- 2022-08-15 CN CN202210974622.9A patent/CN115334448B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107734457A (en) * | 2017-09-29 | 2018-02-23 | 桂林电子科技大学 | Wisdom parking ground navigation system and method |
| WO2019239365A1 (en) * | 2018-06-13 | 2019-12-19 | Purohit Ankit | System and method for position and orientation tracking of multiple mobile devices |
| CN109612466A (en) * | 2018-11-30 | 2019-04-12 | 北斗天地股份有限公司山东分公司 | A kind of automobile-used multi-sensor combined navigation method and system in underground |
| CN109782227A (en) * | 2019-02-20 | 2019-05-21 | 核芯互联科技(青岛)有限公司 | A kind of indoor orientation method based on Bluetooth signal RSSI |
| CN111024075A (en) * | 2019-12-26 | 2020-04-17 | 北京航天控制仪器研究所 | Pedestrian navigation error correction filtering method combining Bluetooth beacon and map |
| WO2021139590A1 (en) * | 2020-01-06 | 2021-07-15 | 三个机器人公司 | Indoor localization and navigation apparatus based on bluetooth and slam, and method therefor |
| CN111681452A (en) * | 2020-01-19 | 2020-09-18 | 重庆大学 | Unmanned vehicle dynamic lane change track planning method based on Frenet coordinate system |
| CN111726757A (en) * | 2020-05-25 | 2020-09-29 | 南京理工大学 | Indoor parking lot positioning and navigation method based on Bluetooth |
| CN114509069A (en) * | 2022-01-25 | 2022-05-17 | 南昌大学 | Indoor navigation positioning system based on Bluetooth AOA and IMU fusion |
| CN115103437A (en) * | 2022-04-27 | 2022-09-23 | 电子科技大学 | Bluetooth and inertial measurement unit tightly-coupled indoor positioning method |
| CN114894185A (en) * | 2022-05-09 | 2022-08-12 | 南昌大学 | Carrier attitude zero-speed correction system based on fusion of Bluetooth AOA and IMU |
Non-Patent Citations (6)
| Title |
|---|
| KATARZYNA FILUS;SŁAWOMIR NOWAK;JOANNA DOMAŃSKA;JAKUB DUDA: "Cost-effective filtering of unreliable proximity detection results based on BLE RSSI and IMU readings using smartphones", Retrieved from the Internet <URL:https://www.nature.com/articles/s41598-022-06201-y> * |
| MOHAMMADAMIN ATASHI: "Multiple Model-based Indoor Localization via Bluetooth Low Energy and Inertial Measurement Unit Sensors", Retrieved from the Internet <URL:《https://spectrum.library.concordia.ca/id/eprint/987720/7/Atashi_MSc_S2021.pdf》> * |
| PAUL K. YOON; SHAGHAYEGH ZIHAJEHZADEH; BONG-SOO KANG; EDWARD J. PARK: "Adaptive Kalman filter for indoor localization using Bluetooth Low Energy and inertial measurement unit", 2015 37TH ANNUAL INTERNATIONAL CONFERENCE OF THE IEEE ENGINEERING IN MEDICINE AND BIOLOGY SOCIETY (EMBC), 5 November 2015 (2015-11-05) * |
| 佚名: "多传感器最优信息融合卡尔曼滤波器", Retrieved from the Internet <URL:《https://zhuanlan.zhihu.com/p/99041293》> * |
| 刘忠志: "基于空间定位模型的三维室内定位系统研究", 中国优秀硕士学位论文全文数据库, 15 January 2021 (2021-01-15) * |
| 赖德朴: "基于蓝牙位置指纹与惯性传感器的室内定位技术研究", 中国优秀硕士学位论文全文数据库, 15 April 2018 (2018-04-15) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115334448B (en) | 2024-03-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110375730B (en) | Indoor positioning navigation system based on IMU and UWB fusion | |
| CN105157697B (en) | Indoor mobile robot pose measurement system and measurement method based on optoelectronic scanning | |
| Fan et al. | Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB | |
| CN105589064B (en) | WLAN location fingerprint database is quickly established and dynamic update system and method | |
| CN206649345U (en) | A kind of Navigation of Pilotless Aircraft device based on ultra-wideband communications | |
| CN107132542B (en) | A kind of small feature loss soft landing autonomic air navigation aid based on optics and Doppler radar | |
| CN110026993B (en) | Human body following robot based on UWB and pyroelectric infrared sensor | |
| CN103197279A (en) | Cooperative location system and location method for moving target | |
| CN108489382A (en) | A kind of AGV dynamic pose measuring methods based on space multi-point constraint | |
| CN106950549A (en) | A kind of Radar Calibration method and system based on less radio-frequency relay transmission technology | |
| CN112967392A (en) | Large-scale park mapping and positioning method based on multi-sensor contact | |
| CN111694001A (en) | Real-time distance measurement positioning system for unmanned aerial vehicle | |
| CN113342059B (en) | Multi-unmanned aerial vehicle tracking mobile radiation source method based on position and speed errors | |
| CN110986952A (en) | High-precision anti-interference indoor positioning method for pedestrians | |
| CN112269202A (en) | Motion carrier assisted space reference transmission system and method | |
| CN114509069B (en) | Indoor navigation positioning system based on Bluetooth AOA and IMU fusion | |
| CN111305859A (en) | Automatic shield tunneling machine guiding system and method based on binocular vision | |
| CN113382354B (en) | Wireless positioning non-line-of-sight signal discrimination method based on factor graph | |
| CN206876184U (en) | A kind of indoor positioning device based on RSSI and inertial navigation | |
| CN113324544A (en) | Indoor mobile robot co-location method based on UWB/IMU (ultra wide band/inertial measurement unit) of graph optimization | |
| CN108445446A (en) | A kind of passive method for locating speed measurement and device | |
| CN113076634A (en) | Multi-machine cooperative passive positioning method, device and system | |
| CN111694006A (en) | Navigation sensing system for indoor unmanned system | |
| CN115334448A (en) | Accurate dynamic positioning method of unmanned self-following device based on Bluetooth and inertial sensor | |
| CN105979581A (en) | Indoor positioning method based on power difference |
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 |