CN113296054A - UWB array-based positioning navigation method and terminal - Google Patents

UWB array-based positioning navigation method and terminal Download PDF

Info

Publication number
CN113296054A
CN113296054A CN202110562532.4A CN202110562532A CN113296054A CN 113296054 A CN113296054 A CN 113296054A CN 202110562532 A CN202110562532 A CN 202110562532A CN 113296054 A CN113296054 A CN 113296054A
Authority
CN
China
Prior art keywords
coordinate
base station
vehicle
distance
uwb
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
Application number
CN202110562532.4A
Other languages
Chinese (zh)
Other versions
CN113296054B (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.)
Jiangsu Shenghai Intelligent Technology Co ltd
Original Assignee
Fujian Shenghai Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Fujian Shenghai Intelligent Technology Co ltd filed Critical Fujian Shenghai Intelligent Technology Co ltd
Priority to CN202110562532.4A priority Critical patent/CN113296054B/en
Publication of CN113296054A publication Critical patent/CN113296054A/en
Application granted granted Critical
Publication of CN113296054B publication Critical patent/CN113296054B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/04—Position of source determined by a plurality of spaced direction-finders

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses a positioning navigation method and a terminal based on a UWB array; the method comprises the steps of selecting a preset UWB base station in a preset UWB array as an origin coordinate to establish a rectangular coordinate system, determining the coordinate of each UWB base station according to the distance between the preset UWB base stations, acquiring the distance between a vehicle and each UWB base station in real time according to a preset time interval, calculating the coordinate of the vehicle according to the coordinates of four UWB base stations closest to the vehicle, and calculating the course angle of the vehicle according to the vehicle coordinates continuously acquired twice, so that the vehicle is positioned and navigated according to the coordinates and the course angle; the location is based on four UWB basic stations with the nearest distance, can make the location more accurate, need not rely on the location navigation of GPS or big dipper simultaneously, and the bandwidth of UWB module is big, and interference immunity is strong, and the signal is more stable.

Description

UWB array-based positioning navigation method and terminal
Technical Field
The invention relates to the technical field of unmanned driving, in particular to a positioning navigation method and a terminal based on a UWB array.
Background
The unmanned automobile is a main trend of future development in the automobile field, and in the unmanned automobile, it is an important link to provide accurate positioning and navigation for the unmanned automobile. Most of the existing positioning and navigation technologies adopted by unmanned driving are GPS or Beidou and the like, but when the signals are shielded or the signals are weak, the vehicle is difficult to position and navigate, so that the unmanned driving task is influenced.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the positioning navigation method and the terminal based on the UWB array are provided, and positioning navigation without depending on GPS or Beidou is realized.
In order to solve the technical problems, the invention adopts the technical scheme that:
a positioning navigation method based on a UWB array comprises the following steps:
s1, selecting a preset UWB base station from a preset UWB array as an origin coordinate to establish a rectangular coordinate system, and determining the coordinate of each UWB base station according to the distance between the preset UWB base stations, wherein the UWB array is a rectangular array erected by a plurality of UWB base stations;
s2, obtaining the distance between the vehicle and each UWB base station in real time according to a preset time interval, and calculating a first coordinate of the current position of the vehicle in the established rectangular coordinate system by combining the coordinates of the four UWB base stations with the shortest distance;
s3, calculating a heading angle of the vehicle according to the first coordinate and a second coordinate of the position of the vehicle before a preset time interval, wherein the heading angle is an included angle between the driving direction of the vehicle and the Y-axis positive direction of the rectangular coordinate system;
and S4, positioning and navigating the vehicle in the rectangular coordinate system according to the first coordinate and the heading angle of the vehicle.
In order to solve the technical problem, the invention adopts another technical scheme as follows:
a positioning and navigation terminal based on UWB array, comprising a processor, a memory and a computer program stored on the memory and operable on the processor, wherein the processor implements the following steps when executing the computer program:
s1, selecting a preset UWB base station from a preset UWB array as an origin coordinate to establish a rectangular coordinate system, and determining the coordinate of each UWB base station according to the distance between the preset UWB base stations, wherein the UWB array is a rectangular array erected by a plurality of UWB base stations;
s2, obtaining the distance between the vehicle and each UWB base station in real time according to a preset time interval, and calculating a first coordinate of the current position of the vehicle in the established rectangular coordinate system by combining the coordinates of the four UWB base stations with the shortest distance;
s3, calculating a heading angle of the vehicle according to the first coordinate and a second coordinate of the position of the vehicle before a preset time interval, wherein the heading angle is an included angle between the driving direction of the vehicle and the Y-axis positive direction of the rectangular coordinate system;
and S4, positioning and navigating the vehicle in the rectangular coordinate system according to the first coordinate and the heading angle of the vehicle.
The invention has the beneficial effects that: according to the invention, a rectangular array is erected by a plurality of UWB base stations, a rectangular coordinate system is established according to the distance between the vehicle-mounted UWB module and the UWB base stations and the fixed distance between the base stations, so that the real-time rapid positioning of the vehicle is realized, and the positioning is based on the four UWB base stations with the closest distances, so that the positioning is more accurate, the course angle of the vehicle can be obtained in real time, the positioning navigation without depending on GPS or Beidou is realized, and the UWB module has large bandwidth, strong anti-interference and more stable signals.
Drawings
Fig. 1 is a flowchart of a positioning navigation method based on UWB array according to an embodiment of the present invention;
FIG. 2 is a block diagram of a positioning navigation terminal based on UWB array according to the embodiment of the invention;
FIG. 3 is a coordinate display diagram of a positioning and navigation method based on UWB array according to the embodiment of the invention;
description of reference numerals:
1. a positioning navigation terminal based on UWB array; 2. a processor; 3. a memory.
Detailed Description
In order to explain technical contents, achieved objects, and effects of the present invention in detail, the following description is made with reference to the accompanying drawings in combination with the embodiments.
Referring to fig. 1 and fig. 3, a positioning navigation method based on a UWB array includes:
s1, selecting a preset UWB base station from a preset UWB array as an origin coordinate to establish a rectangular coordinate system, and determining the coordinate of each UWB base station according to the distance between the preset UWB base stations, wherein the UWB array is a rectangular array erected by a plurality of UWB base stations;
s2, obtaining the distance between the vehicle and each UWB base station in real time according to a preset time interval, and calculating a first coordinate of the current position of the vehicle in the established rectangular coordinate system by combining the coordinates of the four UWB base stations with the shortest distance;
s3, calculating a heading angle of the vehicle according to the first coordinate and a second coordinate of the position of the vehicle before a preset time interval, wherein the heading angle is an included angle between the driving direction of the vehicle and the Y-axis positive direction of the rectangular coordinate system;
and S4, positioning and navigating the vehicle in the rectangular coordinate system according to the first coordinate and the heading angle of the vehicle.
From the above description, the beneficial effects of the present invention are: the invention has the beneficial effects that: according to the invention, a rectangular array is erected by a plurality of UWB base stations, a rectangular coordinate system is established according to the distance between the vehicle-mounted UWB module and the UWB base stations and the fixed distance between the base stations, so that the real-time rapid positioning of the vehicle is realized, and the positioning is based on the four UWB base stations with the closest distances, so that the positioning is more accurate, the course angle of the vehicle can be obtained in real time, the positioning navigation without depending on GPS or Beidou is realized, and the UWB module has large bandwidth, strong anti-interference and more stable signals.
Further, the step S2 is specifically:
obtaining the distance between a vehicle and each UWB base station according to a preset time interval, obtaining the base station coordinates of four UWB base stations with the shortest distance, selecting two base station coordinates with the smallest ordinate as a first base station coordinate and a second base station coordinate, wherein the abscissa of the first base station coordinate is smaller than the abscissa of the second base station coordinate, and substituting a first distance a between the vehicle and the first base station coordinate, a second distance c between the vehicle and the second base station coordinate, and a third distance b between the first base station coordinate and the second base station coordinate into the following formula:
p＝(a+b+c)/2；
S＝√[p*(p-a)*(p-b)*(p-c)]
S＝h*b/2
a2＝h2+t2
and calculating a transverse difference t and a longitudinal difference h of the first coordinate of the current position of the vehicle relative to the first base station coordinate (x, y), so as to obtain the first coordinate (x + t, y + h).
According to the method, the distance between the vehicle and each UWB base station is obtained, two of the four closest base stations are selected to obtain the coordinates of the current vehicle position coordinates, compared with the UWB base stations with the long distance, the distance between the vehicle and the UWB base stations with the short distance can be more accurate, and because the positioning is carried out based on the UWB matrix, the positioning coordinates of the vehicle are all located in the rectangular range surrounded by the four UWB base stations each time, the vehicle positioning range can be rapidly determined, and two coordinates are selected to form a triangle, so that the positioning coordinates of the vehicle can be calculated more conveniently and rapidly, and the accuracy and the convenience for obtaining the vehicle coordinates are improved.
Further, the step S2 further includes:
and if the difference value between the sum of the first distance and the second distance and the third distance is smaller than a preset distance error value, the first coordinate of the current position of the vehicle is located between the first base station coordinate and the second base station coordinate, and the first coordinate of the current position of the vehicle is calculated according to the first base station coordinate and the first distance.
From the above description, it can be determined whether the vehicle is located on the connection line between the two base stations according to the relationship among the first distance, the second distance, and the third distance, and at this time, the vehicle can be located more quickly according to the distance between the vehicle and the base station and the coordinates of the base station.
Further, the step S2 further includes:
and judging whether the difference values of the first distance, the second distance and the third distance are smaller than a preset distance error value, if so, the first coordinate is the first base station coordinate.
As can be seen from the above description, it can be determined whether the vehicle is located at the base station according to the relationship among the first distance, the second distance, and the third distance, and the coordinates of the base station, that is, the coordinates of the vehicle at this time.
Further, the step S3 is specifically:
calculating a vehicle driving vector Vc (x1-x0, y1-y0) according to the first coordinates (x1, y1) of the vehicle and second coordinates (x0, y0) of the position before a preset time interval; and the positive vector Vy of the Y axis is (0,1) according to the following formula:
θ＝arccos(Vc*Vy/(|Vc|*|Vy|)
an included angle θ is calculated, the heading angle is θ if x1-x0>0, the heading angle is pi- θ if x1-x0<0, the heading angle is 0 if x1-x0 is 0 and y1-y0> is 0, and the heading angle is pi if x1-x0 is 0 and y1-y0< 0.
According to the description, the included angle between the vehicle running direction and the positive direction of the Y axis is used as the heading angle, so that a good reference can be formed, calculation is facilitated, the running vector of the vehicle is obtained through the two-time positioning coordinates of the vehicle, the heading angle of the vehicle can be calculated according to the positive vector of the Y axis, and the calculation is more reasonable and accurate.
Referring to fig. 2, a positioning navigation terminal based on UWB array includes a processor, a memory and a computer program stored in the memory and operable on the processor, wherein the processor implements the following steps when executing the computer program:
s1, selecting a preset UWB base station from a preset UWB array as an origin coordinate to establish a rectangular coordinate system, and determining the coordinate of each UWB base station according to the distance between the preset UWB base stations, wherein the UWB array is a rectangular array erected by a plurality of UWB base stations;
s2, obtaining the distance between the vehicle and each UWB base station in real time according to a preset time interval, and calculating a first coordinate of the current position of the vehicle in the established rectangular coordinate system by combining the coordinates of the four UWB base stations with the shortest distance;
s3, calculating a heading angle of the vehicle according to the first coordinate and a second coordinate of the position of the vehicle before a preset time interval, wherein the heading angle is an included angle between the driving direction of the vehicle and the Y-axis positive direction of the rectangular coordinate system;
and S4, positioning and navigating the vehicle in the rectangular coordinate system according to the first coordinate and the heading angle of the vehicle.
From the above description, the beneficial effects of the present invention are: the invention has the beneficial effects that: according to the invention, a rectangular array is erected by a plurality of UWB base stations, a rectangular coordinate system is established according to the distance between the vehicle-mounted UWB module and the UWB base stations and the fixed distance between the base stations, so that the real-time rapid positioning of the vehicle is realized, and the positioning is based on the four UWB base stations with the closest distances, so that the positioning is more accurate, the course angle of the vehicle can be obtained in real time, the positioning navigation without depending on GPS or Beidou is realized, and the UWB module has large bandwidth, strong anti-interference and more stable signals.
Further, the step S2 is specifically:
obtaining the distance between a vehicle and each UWB base station according to a preset time interval, obtaining the base station coordinates of four UWB base stations with the shortest distance, selecting two base station coordinates with the smallest ordinate as a first base station coordinate and a second base station coordinate, wherein the abscissa of the first base station coordinate is smaller than the abscissa of the second base station coordinate, and substituting a first distance a between the vehicle and the first base station coordinate, a second distance c between the vehicle and the second base station coordinate, and a third distance b between the first base station coordinate and the second base station coordinate into the following formula:
p＝(a+b+c)/2；
S＝√[p*(p-a)*(p-b)*(p-c)]
S＝h*b/2
a2＝h2+t2
and calculating a transverse difference t and a longitudinal difference h of the first coordinate of the current position of the vehicle relative to the first base station coordinate (x, y), so as to obtain the first coordinate (x + t, y + h).
According to the method, the distance between the vehicle and each UWB base station is obtained, two of the four closest base stations are selected to obtain the coordinates of the current vehicle position coordinates, compared with the UWB base stations with the long distance, the distance between the vehicle and the UWB base stations with the short distance can be more accurate, and because the positioning is carried out based on the UWB matrix, the positioning coordinates of the vehicle are all located in the rectangular range surrounded by the four UWB base stations each time, the vehicle positioning range can be rapidly determined, and two coordinates are selected to form a triangle, so that the positioning coordinates of the vehicle can be calculated more conveniently and rapidly, and the accuracy and the convenience for obtaining the vehicle coordinates are improved.
Further, when the processor executes the computer program, the step S2 further includes:
and if the difference value between the sum of the first distance and the second distance and the third distance is smaller than a preset distance error value, the first coordinate of the current position of the vehicle is located between the first base station coordinate and the second base station coordinate, and the first coordinate of the current position of the vehicle is calculated according to the first base station coordinate and the first distance.
From the above description, it can be determined whether the vehicle is located on the connection line between the two base stations according to the relationship among the first distance, the second distance, and the third distance, and at this time, the vehicle can be located more quickly according to the distance between the vehicle and the base station and the coordinates of the base station.
Further, when the processor executes the computer program, the step S2 further includes:
and judging whether the difference values of the first distance, the second distance and the third distance are smaller than a preset distance error value, if so, the first coordinate is the first base station coordinate.
As can be seen from the above description, it can be determined whether the vehicle is located at the base station according to the relationship among the first distance, the second distance, and the third distance, and the coordinates of the base station, that is, the coordinates of the vehicle at this time.
Further, the step S3 is specifically:
calculating a vehicle driving vector Vc (x1-x0, y1-y0) according to the first coordinates (x1, y1) of the vehicle and second coordinates (x0, y0) of the position before a preset time interval; and the positive vector Vy of the Y axis is (0,1) according to the following formula:
θ＝arccos(Vc*Vy/(|Vc|*|Vy|)
an included angle θ is calculated, the heading angle is θ if x1-x0>0, the heading angle is pi- θ if x1-x0<0, the heading angle is 0 if x1-x0 is 0 and y1-y0> is 0, and the heading angle is pi if x1-x0 is 0 and y1-y0< 0.
According to the description, the included angle between the vehicle running direction and the positive direction of the Y axis is used as the heading angle, so that a good reference can be formed, calculation is facilitated, the running vector of the vehicle is obtained through the two-time positioning coordinates of the vehicle, the heading angle of the vehicle can be calculated according to the positive vector of the Y axis, and the calculation is more reasonable and accurate.
Referring to fig. 1 and fig. 3, a first embodiment of the present invention is:
a positioning navigation method based on a UWB array comprises the following steps:
s1, selecting a preset UWB base station from a preset UWB array as an origin coordinate to establish a rectangular coordinate system, and determining the coordinate of each UWB base station according to the distance between the preset UWB base stations, wherein the UWB array is a rectangular array erected by a plurality of UWB base stations;
in this embodiment, as shown in fig. 3, a 3 × 3 UWB array is established in a preset area by 9 UWB base stations in advance, the distance between the base stations is b (b >0), a rectangular coordinate system is established with the position of one of the base stations as an origin, and the base station coordinates of each base station are calculated.
S2, obtaining the distance between the vehicle and each UWB base station in real time according to a preset time interval, and calculating a first coordinate of the current position of the vehicle in the established rectangular coordinate system by combining the coordinates of the four UWB base stations with the shortest distance;
the step S2 specifically includes:
obtaining the distance between a vehicle and each UWB base station according to a preset time interval, obtaining the base station coordinates of four UWB base stations with the shortest distance, selecting two base station coordinates with the smallest ordinate as a first base station coordinate and a second base station coordinate, wherein the abscissa of the first base station coordinate is smaller than the abscissa of the second base station coordinate, and substituting a first distance a between the vehicle and the first base station coordinate, a second distance c between the vehicle and the second base station coordinate, and a third distance b between the first base station coordinate and the second base station coordinate into the following formula:
p＝(a+b+c)/2；
S＝√[p*(p-a)*(p-b)*(p-c)]
S＝h*b/2
a2＝h2+t2
and calculating a transverse difference t and a longitudinal difference h of the first coordinate of the current position of the vehicle relative to the first base station coordinate (x, y), so as to obtain the first coordinate (x + t, y + h).
In addition, when the difference value between the sum of the first distance and the second distance and the third distance is smaller than a preset distance error value, the first coordinate of the current position of the vehicle is located between the first base station coordinate and the second base station coordinate, and the first coordinate of the current position of the vehicle is calculated according to the first base station coordinate and the first distance;
when the difference values of the first distance, the second distance and the third distance are both smaller than a preset distance error value, the vehicle is considered to be located at a first base station, and the first coordinate is a first base station coordinate;
in this embodiment, as shown in fig. 3, the coordinates of four base stations are (0, 0), (b, 0) and (b, b), it is known that the vehicle is located in a rectangular area surrounded by the four base stations, and the coordinates of the first base station are (0, 0), and the coordinates of the second base station are (b, 0), and the formula is substituted according to a first distance a between the vehicle and the coordinates of the first base station, a second distance c between the vehicle and the coordinates of the second base station, and a third distance b between the coordinates of the first base station and the coordinates of the second base station:
p＝(a+b+c)/2；
S＝√[p*(p-a)*(p-b)*(p-c)]；
S＝h*b/2
a2＝h2+t2
further, it can be calculated that:
h＝2S/b；
t＝√(a2-h2)；
the horizontal coordinate difference t and the vertical coordinate difference h of the vehicle with respect to the first base station coordinate (0, 0) are obtained, and the first coordinate of the vehicle can be obtained as (t, h).
S3, calculating a heading angle of the vehicle according to the first coordinate and a second coordinate of the position of the vehicle before a preset time interval, wherein the heading angle is an included angle between the driving direction of the vehicle and the Y-axis positive direction of the rectangular coordinate system;
the step S3 specifically includes:
calculating a vehicle driving vector Vc (x1-x0, y1-y0) according to the first coordinates (x1, y1) of the vehicle and second coordinates (x0, y0) of the position before a preset time interval; and the positive vector Vy of the Y axis is (0,1) according to the following formula:
θ＝arccos(Vc*Vy/(|Vc|*|Vy|)
an included angle θ is calculated, the heading angle is θ if x1-x0>0, the heading angle is pi- θ if x1-x0<0, the heading angle is 0 if x1-x0 is 0 and y1-y0> is 0, and the heading angle is pi if x1-x0 is 0 and y1-y0< 0.
In this embodiment, the first coordinate of the vehicle is (t, h), and the second coordinate before the preset time interval is (0, 0), so the vehicle form vector Vc is (t, h), and the formula is substituted with Vy being (0, 1):
θ＝arccos(Vc*Vy/(|Vc|*|Vy|)
the available theta is arccos [ h/√ (h)2+t2)]＝arccos(h/a)。
And S4, positioning and navigating the vehicle in the rectangular coordinate system according to the first coordinate and the heading angle of the vehicle.
Referring to fig. 2, the second embodiment of the present invention is:
a positioning navigation terminal 1 based on UWB array comprises a processor 2, a memory 3 and a computer program stored on the memory 3 and capable of running on the processor 2, wherein the processor 2 realizes the steps of the first embodiment when executing the computer program.
In summary, according to the positioning and navigation method and terminal based on the UWB array provided by the invention, the rectangular array is erected by the plurality of UWB base stations, the rectangular coordinate system is established by the rectangular array, and the vehicle is quickly positioned in real time according to the distance between the vehicle-mounted UWB module and the UWB base stations and the fixed distance between the base stations.
The above description is only an embodiment of the present invention, and not intended to limit the scope of the present invention, and all equivalent changes made by using the contents of the present specification and the drawings, or applied directly or indirectly to the related technical fields, are included in the scope of the present invention.

Claims (10)

1. A positioning navigation method based on UWB array is characterized in that the method comprises the following steps:
s1, selecting a preset UWB base station from a preset UWB array as an origin coordinate to establish a rectangular coordinate system, and determining the coordinate of each UWB base station according to the distance between the preset UWB base stations, wherein the UWB array is a rectangular array erected by a plurality of UWB base stations;
s2, obtaining the distance between the vehicle and each UWB base station in real time according to a preset time interval, and calculating a first coordinate of the current position of the vehicle in the established rectangular coordinate system by combining the coordinates of the four UWB base stations with the shortest distance;
s3, calculating a heading angle of the vehicle according to the first coordinate and a second coordinate of the position of the vehicle before a preset time interval, wherein the heading angle is an included angle between the driving direction of the vehicle and the Y-axis positive direction of the rectangular coordinate system;
and S4, positioning and navigating the vehicle in the rectangular coordinate system according to the first coordinate and the heading angle of the vehicle.
2. The positioning and navigation method based on the UWB array according to claim 1, wherein the step S2 specifically comprises:
obtaining the distance between a vehicle and each UWB base station according to a preset time interval, obtaining the base station coordinates of four UWB base stations with the shortest distance, selecting two base station coordinates with the smallest ordinate as a first base station coordinate and a second base station coordinate, wherein the abscissa of the first base station coordinate is smaller than the abscissa of the second base station coordinate, and substituting a first distance a between the vehicle and the first base station coordinate, a second distance c between the vehicle and the second base station coordinate, and a third distance b between the first base station coordinate and the second base station coordinate into the following formula:
p＝(a+b+c)/2；
S＝√[p*(p-a)*(p-b)*(p-c)]
S＝h*b/2
a2＝h2+t2
and calculating a transverse difference t and a longitudinal difference h of the first coordinate of the current position of the vehicle relative to the first base station coordinate (x, y), so as to obtain the first coordinate (x + t, y + h).
3. The UWB array-based positioning and navigation method according to claim 2, wherein the step S2 further comprises:
and if the difference value between the sum of the first distance and the second distance and the third distance is smaller than a preset distance error value, the first coordinate of the current position of the vehicle is located between the first base station coordinate and the second base station coordinate, and the first coordinate of the current position of the vehicle is calculated according to the first base station coordinate and the first distance.
4. The UWB array-based positioning and navigation method according to claim 3, wherein the step S2 further comprises:
and judging whether the difference values of the first distance, the second distance and the third distance are smaller than a preset distance error value, if so, the first coordinate is the first base station coordinate.
5. The positioning and navigation method based on the UWB array according to claim 1, wherein the step S3 specifically comprises:
calculating a vehicle driving vector Vc (x1-x0, y1-y0) according to the first coordinates (x1, y1) of the vehicle and second coordinates (x0, y0) of the position before a preset time interval; and the positive vector Vy of the Y axis is (0,1) according to the following formula:
θ＝arccos(Vc*Vy/(|Vc|*|Vy|)
an included angle θ is calculated, the heading angle is θ if x1-x0>0, the heading angle is pi- θ if x1-x0<0, the heading angle is 0 if x1-x0 is 0 and y1-y0> is 0, and the heading angle is pi if x1-x0 is 0 and y1-y0< 0.
6. A positioning and navigation terminal based on UWB array, comprising a processor, a memory and a computer program stored in the memory and operable on the processor, wherein the processor executes the computer program to implement the following steps:
s1, selecting a preset UWB base station from a preset UWB array as an origin coordinate to establish a rectangular coordinate system, and determining the coordinate of each UWB base station according to the distance between the preset UWB base stations, wherein the UWB array is a rectangular array erected by a plurality of UWB base stations;
s2, obtaining the distance between the vehicle and each UWB base station in real time according to a preset time interval, and calculating a first coordinate of the current position of the vehicle in the established rectangular coordinate system by combining the coordinates of the four UWB base stations with the shortest distance;
s3, calculating a heading angle of the vehicle according to the first coordinate and a second coordinate of the position of the vehicle before a preset time interval, wherein the heading angle is an included angle between the driving direction of the vehicle and the Y-axis positive direction of the rectangular coordinate system;
and S4, positioning and navigating the vehicle in the rectangular coordinate system according to the first coordinate and the heading angle of the vehicle.
7. The positioning and navigation terminal based on the UWB array according to claim 6, wherein the step S2 is specifically to:
obtaining the distance between a vehicle and each UWB base station according to a preset time interval, obtaining the base station coordinates of four UWB base stations with the shortest distance, selecting two base station coordinates with the smallest ordinate as a first base station coordinate and a second base station coordinate, wherein the abscissa of the first base station coordinate is smaller than the abscissa of the second base station coordinate, and substituting a first distance a between the vehicle and the first base station coordinate, a second distance c between the vehicle and the second base station coordinate, and a third distance b between the first base station coordinate and the second base station coordinate into the following formula:
p＝(a+b+c)/2；
S＝√[p*(p-a)*(p-b)*(p-c)]
S＝h*b/2
a2＝h2+t2
and calculating a transverse difference t and a longitudinal difference h of the first coordinate of the current position of the vehicle relative to the first base station coordinate (x, y), so as to obtain the first coordinate (x + t, y + h).
8. The UWB array-based positioning and navigation terminal according to claim 7, wherein the processor, when executing the computer program, further comprises the step S2 of:
and if the difference value between the sum of the first distance and the second distance and the third distance is smaller than a preset distance error value, the first coordinate of the current position of the vehicle is located between the first base station coordinate and the second base station coordinate, and the first coordinate of the current position of the vehicle is calculated according to the first base station coordinate and the first distance.
9. The UWB array-based positioning and navigation terminal according to claim 8, wherein the processor, when executing the computer program, further comprises the step S2 of:
and judging whether the difference values of the first distance, the second distance and the third distance are smaller than a preset distance error value, if so, the first coordinate is the first base station coordinate.
10. The positioning and navigation terminal based on the UWB array according to claim 6, wherein the step S3 is specifically to:
calculating a vehicle driving vector Vc (x1-x0, y1-y0) according to the first coordinates (x1, y1) of the vehicle and second coordinates (x0, y0) of the position before a preset time interval; and the positive vector Vy of the Y axis is (0,1) according to the following formula:
θ＝arccos(Vc*Vy/(|Vc|*|Vy|)
an included angle θ is calculated, the heading angle is θ if x1-x0>0, the heading angle is pi- θ if x1-x0<0, the heading angle is 0 if x1-x0 is 0 and y1-y0> is 0, and the heading angle is pi if x1-x0 is 0 and y1-y0< 0.
CN202110562532.4A 2021-05-24 2021-05-24 Positioning navigation method and terminal based on UWB array Active CN113296054B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110562532.4A CN113296054B (en) 2021-05-24 2021-05-24 Positioning navigation method and terminal based on UWB array

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110562532.4A CN113296054B (en) 2021-05-24 2021-05-24 Positioning navigation method and terminal based on UWB array

Publications (2)

Publication Number Publication Date
CN113296054A true CN113296054A (en) 2021-08-24
CN113296054B CN113296054B (en) 2024-01-26

Family

ID=77324075

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110562532.4A Active CN113296054B (en) 2021-05-24 2021-05-24 Positioning navigation method and terminal based on UWB array

Country Status (1)

Country Link
CN (1) CN113296054B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113747354A (en) * 2021-09-03 2021-12-03 上海博泰悦臻电子设备制造有限公司 Method for vehicle positioning, vehicle-mounted UWB device, roadside UWB device, and medium
CN114173412A (en) * 2021-11-30 2022-03-11 广州致远电子有限公司 Vehicle positioning method and device based on UWB vehicle-mounted key
WO2023116645A1 (en) * 2021-12-21 2023-06-29 北京罗克维尔斯科技有限公司 Mobile terminal positioning method and apparatus, device and storage medium

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5722083A (en) * 1995-07-08 1998-02-24 Alcatel N.V. Directing a subscriber toward a destination within an SDMA mobile radio network
CN107578482A (en) * 2017-07-21 2018-01-12 上海电科市政工程有限公司 Quick highway toll collection system and method based on technology of Internet of things
CN108827292A (en) * 2018-06-27 2018-11-16 四川大学 The accurate method for locating speed measurement of combined navigation and system based on GNSS and ground base station
CN109341683A (en) * 2018-06-29 2019-02-15 中国人民解放军海军工程大学 Course based on the bis- labels of UWB calculates and its method for analyzing performance
CN109471066A (en) * 2018-11-15 2019-03-15 奇瑞汽车股份有限公司 The determination method, apparatus and vehicle of vehicle course angle
WO2020027496A1 (en) * 2018-08-03 2020-02-06 Lg Electronics Inc. Moving robot and controlling method thereof
CN111661061A (en) * 2020-06-18 2020-09-15 东软睿驰汽车技术（沈阳）有限公司 Vehicle driving intention identification method and device

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5722083A (en) * 1995-07-08 1998-02-24 Alcatel N.V. Directing a subscriber toward a destination within an SDMA mobile radio network
CN107578482A (en) * 2017-07-21 2018-01-12 上海电科市政工程有限公司 Quick highway toll collection system and method based on technology of Internet of things
CN108827292A (en) * 2018-06-27 2018-11-16 四川大学 The accurate method for locating speed measurement of combined navigation and system based on GNSS and ground base station
CN109341683A (en) * 2018-06-29 2019-02-15 中国人民解放军海军工程大学 Course based on the bis- labels of UWB calculates and its method for analyzing performance
WO2020027496A1 (en) * 2018-08-03 2020-02-06 Lg Electronics Inc. Moving robot and controlling method thereof
CN109471066A (en) * 2018-11-15 2019-03-15 奇瑞汽车股份有限公司 The determination method, apparatus and vehicle of vehicle course angle
CN111661061A (en) * 2020-06-18 2020-09-15 东软睿驰汽车技术（沈阳）有限公司 Vehicle driving intention identification method and device

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113747354A (en) * 2021-09-03 2021-12-03 上海博泰悦臻电子设备制造有限公司 Method for vehicle positioning, vehicle-mounted UWB device, roadside UWB device, and medium
CN114173412A (en) * 2021-11-30 2022-03-11 广州致远电子有限公司 Vehicle positioning method and device based on UWB vehicle-mounted key
CN114173412B (en) * 2021-11-30 2023-05-12 广州致远电子股份有限公司 Vehicle positioning method and device based on UWB vehicle-mounted key
WO2023116645A1 (en) * 2021-12-21 2023-06-29 北京罗克维尔斯科技有限公司 Mobile terminal positioning method and apparatus, device and storage medium

Also Published As

Publication number Publication date
CN113296054B (en) 2024-01-26

Similar Documents

Publication Publication Date Title
CN113296054A (en) UWB array-based positioning navigation method and terminal
CN109074668B (en) Path navigation method, related device and computer readable storage medium
CN109974727B (en) Robot charging method and device and robot
CN105865461B (en) A kind of car position system and method based on Multi-sensor Fusion algorithm
CN110161510B (en) Obstacle positioning method and device based on ultrasonic radar
US8170797B2 (en) Method and apparatus for estimating behaviors of vehicle using GPS signals
CN112684432B (en) Laser radar calibration method, device, equipment and storage medium
WO2019188745A1 (en) Information processing device, control method, program, and storage medium
CN111650598A (en) External parameter calibration method and device for vehicle-mounted laser scanning system
CN109910008B (en) Obstacle avoidance early warning system and method for data type laser radar robot
CN111753605A (en) Lane line positioning method and device, electronic equipment and readable medium
WO2024021656A1 (en) Positioning system and method, and storage medium
CN113740871A (en) Laser SLAM method, system equipment and storage medium in high dynamic environment
CN113295159B (en) Positioning method and device for end cloud integration and computer readable storage medium
CN113296120B (en) Obstacle detection method and terminal
CN111638487B (en) Automatic parking test equipment and method
CN110909456B (en) Modeling method, modeling device, terminal equipment and medium
CN108454619B (en) Driving assistance method and system
CN111141252B (en) Monocular calibration ranging method and system
CN113740876B (en) Three-dimensional laser radar light path adjusting method and device and electronic equipment
JPH03108609A (en) Current position displaying apparatus for moving object
CN113916253A (en) Method for determining covariance and related device
CN113271547A (en) Unmanned tracking driving method and terminal based on UWB array positioning navigation
CN111965677A (en) Positioning method, device and equipment
CN114623834B (en) Combined navigation positioning measurement method and measurement system

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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20231227

Address after: No. 188 Nanhuan Road, Tangqiao Town, Zhangjiagang City, Suzhou City, Jiangsu Province, 215614

Applicant after: JIANGSU SHENGHAI INTELLIGENT TECHNOLOGY Co.,Ltd.

Address before: 350200 Building 2, phase II, R & D building, Southeast China big data Industrial Park, 2 Hujiang Road, Wenwusha Town, Changle District, Fuzhou City, Fujian Province

Applicant before: Fujian Shenghai Intelligent Technology Co.,Ltd.

GR01 Patent grant
GR01 Patent grant