CN113670296B - Method and device for generating environment map based on ultrasonic waves - Google Patents
Method and device for generating environment map based on ultrasonic waves Download PDFInfo
- Publication number
- CN113670296B CN113670296B CN202110946715.6A CN202110946715A CN113670296B CN 113670296 B CN113670296 B CN 113670296B CN 202110946715 A CN202110946715 A CN 202110946715A CN 113670296 B CN113670296 B CN 113670296B
- Authority
- CN
- China
- Prior art keywords
- ultrasonic measurement
- current
- measurement value
- conflict
- ultrasonic
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000005259 measurement Methods 0.000 claims abstract description 370
- 238000006073 displacement reaction Methods 0.000 claims description 38
- 238000002604 ultrasonography Methods 0.000 claims description 20
- 238000001514 detection method Methods 0.000 claims description 10
- 230000004888 barrier function Effects 0.000 claims description 6
- 238000000926 separation method Methods 0.000 claims description 5
- 230000007613 environmental effect Effects 0.000 claims 1
- 230000008569 process Effects 0.000 abstract description 5
- 238000005516 engineering process Methods 0.000 description 12
- 238000010586 diagram Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000036544 posture Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012806 monitoring device Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3837—Data obtained from a single source
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
- G01S15/89—Sonar systems specially adapted for specific applications for mapping or imaging
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Acoustics & Sound (AREA)
- Computer Networks & Wireless Communication (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
The invention discloses an environment map generation method and device based on ultrasonic waves, wherein the method comprises the following steps: judging whether a plurality of ultrasonic measurement values conflict or not; when a conflict exists, forming a conflict sequence by at least two ultrasonic measurement values with the conflict, and initializing the confidence of each ultrasonic measurement value in the conflict sequence; updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value, so as to obtain the target confidence coefficient of the current ultrasonic measurement value; removing the ultrasonic measurement value with the minimum target confidence coefficient in the conflict sequence; circularly executing the process until no conflict exists in the rest ultrasonic measured values to obtain a target ultrasonic measured value; and updating the historical environment map according to the current pose information and the target ultrasonic measured value determined by the pose information at the moment on the vehicle and the current running state of the vehicle.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to an environment map generation method and device based on ultrasonic waves.
Background
The technology relies on cooperation of artificial intelligence, visual computing, radar, monitoring device and global positioning system, so that the computer can operate the motor vehicle automatically and safely without any active operation of human beings. The path planning technology is one of core technologies of an automatic driving vehicle, and if a better path is planned, the path planning technology needs to be combined with an environment map to judge obstacle information around the vehicle.
In the related art, technologies adopted for creating an environment map mainly include an ultrasonic technology, a vision sensor technology and a laser radar technology. Since both vision sensor technology and lidar technology require high costs, ultrasonic technology is widely used at present. Specifically, at least one ultrasonic sensor may be installed on the vehicle, an obstacle around the vehicle may be detected using the ultrasonic sensor, and an environment map may be created or updated according to the obstacle detection result. However, there may be interference between ultrasonic signals measured by the same ultrasonic sensor and between ultrasonic signals measured by different ultrasonic sensors, which may result in a low accuracy of the generated environment map.
Disclosure of Invention
The invention provides an environment map generation method and device based on ultrasonic waves, which are used for improving the accuracy of an environment map. The specific technical scheme is as follows:
in a first aspect, an embodiment of the present invention provides an ultrasound-based environment map generating method, including:
judging whether the ultrasonic measured values have conflict or not according to obstacle measured results represented by the ultrasonic measured values in a grid map;
when the plurality of ultrasonic measurement values conflict, forming a conflict sequence by at least two ultrasonic measurement values with conflicts, and initializing the confidence of each ultrasonic measurement value in the conflict sequence, wherein each ultrasonic measurement value in the conflict sequence conflicts with at least one other ultrasonic measurement value in the conflict sequence;
updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value to obtain the target confidence coefficient of the current ultrasonic measurement value;
Removing the ultrasonic measurement value with the minimum target confidence coefficient in the conflict sequence, returning to the execution step, judging whether the plurality of ultrasonic measurement values conflict or not according to the obstacle measurement results represented by the plurality of ultrasonic measurement values in the grid map, and determining the current remaining ultrasonic measurement values as target ultrasonic measurement values when no conflict exists in the current remaining ultrasonic measurement values;
and updating the historical environment map according to the current pose information determined by the pose information at the moment on the vehicle and the current running state of the vehicle and the target ultrasonic measured value.
Optionally, updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the collision sequence and the initialization confidence coefficient of the ultrasonic measurement value having a collision with the current ultrasonic measurement value, to obtain the target confidence coefficient of the current ultrasonic measurement value, including:
iteratively updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value;
And when the difference between the confidence coefficient of each ultrasonic measurement value after the latest two iterative updating in the conflict sequence is smaller than a preset difference threshold value, determining the confidence coefficient of the current ultrasonic measurement value after the latest iterative updating as the target confidence coefficient.
Optionally, performing iterative updating on the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the collision sequence and the initialization confidence coefficient of the ultrasonic measurement value having a collision with the current ultrasonic measurement value, including:
substituting the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value into an iterative formula, and carrying out iterative updating on the confidence coefficient of the current ultrasonic measurement value;
the iterative formula is as follows: p (P) k+1 (i)＝(1-Π j∈S P k (j))+(Π j∈S P k (j))P k (i)，
Wherein i represents an ith ultrasonic measurement value in the conflict sequence, S represents an ultrasonic measurement value set in the conflict sequence, which conflicts with the ith ultrasonic measurement value, j represents a jth ultrasonic measurement value in the ultrasonic measurement value set, P represents a confidence level, the confidence level when k=1 is an initialization confidence level, and k is increased by 1 every iteration.
Optionally, determining the current pose information from the pose information of the vehicle at the previous moment and the current running state of the vehicle includes:
acquiring pose information of the previous moment, a current steering wheel rotation angle of the vehicle, left driven wheel displacement of the current moment relative to the previous moment, right driven wheel displacement of the current moment relative to the previous moment, front and rear wheel wheelbases and a rotation angle ratio of a front wheel and a steering wheel at the current moment;
and determining the current pose information according to the pose information at the last moment, the current steering wheel corner, the left driven wheel displacement, the right driven wheel displacement, the front-rear wheel distance and the corner ratio.
Optionally, determining the current pose information according to the pose information at the previous moment, the current steering wheel angle, the left driven wheel displacement, the right driven wheel displacement, the front-rear wheel distance and the angle ratio includes:
calculating the current pose information by the following formula:
wherein,representing the current pose information, +.>Pose information representing the last moment,
x represents the abscissa of the midpoint of the two rear wheel midpoint connecting lines, y represents the ordinate of the midpoint of the two rear wheel midpoint connecting lines, θ represents the heading angle of the vehicle, Δs represents the equivalent driven shaft speed, Δθ represents the heading angle variation, Δw cl Indicating the displacement of the left driven wheel relative to the previous moment, deltaW cr Representing the displacement of the right driven wheel of the current moment relative to the previous moment, s W Indicating the current steering wheel angle, i S Representing the rotation angle ratio, W B Representing the front-rear wheel separation.
Optionally determining whether there is a collision in the plurality of ultrasonic measurements based on obstacle measurements characterized by the plurality of ultrasonic measurements in a grid map, including:
judging whether two ultrasonic measured values in the plurality of ultrasonic measured values have preset conflict in the grid map to determine an overlapping area;
if the preset conflict determination overlapping area exists, judging whether barrier measurement results of the two ultrasonic measurement values in the preset conflict determination overlapping area are the same or not;
if not, it is determined that there is a conflict between the two ultrasonic measurements.
Optionally, updating the historical environment map according to the current pose information determined by the pose information at the moment on the vehicle and the current running state of the vehicle and the target ultrasonic measurement value, including:
for the same grid to be updated in the historical environment map, if all the target ultrasonic measured values covering the grid to be updated determine that an obstacle exists at the grid to be updated, marking the grid to be updated as the obstacle;
If all the target ultrasonic measurement values covering the grid to be updated do not determine that the obstacle exists at the grid to be updated, marking the grid to be updated as the obstacle does not exist;
wherein the grid to be updated comprises a grid covered by a target ultrasonic measured value in grids which are not detected by an obstacle in the historical environment map;
and updating the pose of the vehicle in the historical environment map according to the current pose information.
Optionally, updating the historical environment map according to the current pose information determined by the pose information at the moment on the vehicle and the current running state of the vehicle and the target ultrasonic measurement value, and further comprising:
and expanding the edge area in front of the vehicle in the historical environment map according to the current pose information, and deleting the edge area in back of the vehicle so as to ensure that the size of the updated historical environment map is unchanged.
Optionally, the method further comprises:
and storing each grid in the updated historical environment map in a three-value form, wherein the three values comprise obstacles, no obstacles and no ultrasonic detection.
In a second aspect, an embodiment of the present invention provides an ultrasound-based environment map generating apparatus, including:
A judging unit for judging whether the plurality of ultrasonic measurement values have conflict according to the obstacle measurement results represented by the plurality of ultrasonic measurement values in the grid map;
a generating unit, configured to, when there is a conflict in the plurality of ultrasonic measurement values, configure at least two ultrasonic measurement values that have a conflict into a conflict sequence, where each ultrasonic measurement value in the conflict sequence has a conflict with at least one other ultrasonic measurement value in the conflict sequence;
an initializing unit, configured to initialize a confidence level of each ultrasonic measurement value in the collision sequence;
the confidence coefficient updating unit is used for updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value to obtain the target confidence coefficient of the current ultrasonic measurement value;
the removing unit is used for removing the ultrasonic measurement value with the minimum target confidence coefficient in the conflict sequence, returning to the judging unit, and executing the step of judging whether the plurality of ultrasonic measurement values conflict or not according to the obstacle measurement results represented by the plurality of ultrasonic measurement values in the grid map;
A determining unit configured to determine, when there is no conflict in the current remaining ultrasonic measurement values, the current remaining ultrasonic measurement values as target ultrasonic measurement values;
and the map updating unit is used for updating the historical environment map according to the current pose information determined by the pose information at the moment on the vehicle and the current running state of the vehicle and the target ultrasonic measured value.
Optionally, the confidence updating unit includes:
the iteration updating module is used for carrying out iteration updating on the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value;
and the confidence coefficient determining module is used for determining the confidence coefficient of the current ultrasonic measurement value after the last iteration update as the target confidence coefficient when the difference between the confidence coefficient of each ultrasonic measurement value after the last iteration update of the two times in the conflict sequence is smaller than a preset difference threshold value.
Optionally, the iteration updating module is configured to substitute the initialization confidence coefficient of the current ultrasonic measurement value in the collision sequence and the initialization confidence coefficient of the ultrasonic measurement value that collides with the current ultrasonic measurement value into an iteration formula, and iteratively update the confidence coefficient of the current ultrasonic measurement value;
The iterative formula is as follows: p (P) k+1 (i)＝(1-Π j∈S P k (j))+(Π j∈S P k (j))P k (i)，
Wherein i represents an ith ultrasonic measurement value in the conflict sequence, S represents an ultrasonic measurement value set in the conflict sequence, which conflicts with the ith ultrasonic measurement value, j represents a jth ultrasonic measurement value in the ultrasonic measurement value set, P represents a confidence level, the confidence level when k=1 is an initialization confidence level, and k is increased by 1 every iteration.
Optionally, the device further comprises a pose determining unit, which is used for determining current pose information according to pose information of the vehicle at the moment and the current running state of the vehicle;
the pose determination unit includes:
the acquisition module is used for acquiring pose information of the previous moment, the current steering wheel rotation angle of the vehicle, left driven wheel displacement of the current moment relative to the previous moment, right driven wheel displacement of the current moment relative to the previous moment, wheel base of front wheels and rear wheels and the rotation angle ratio of the front wheels and the steering wheel at the current moment;
and the pose determining module is used for determining the current pose information according to the pose information at the last moment, the current steering wheel rotation angle, the left driven wheel displacement, the right driven wheel displacement, the front and rear wheel distances and the rotation angle ratio.
Optionally, the pose determining module is configured to calculate the current pose information according to the following formula:
wherein,representing the current pose information, +.>Pose information representing the last moment,
x represents the abscissa of the midpoint of the two rear wheel midpoint connecting lines, y represents the ordinate of the midpoint of the two rear wheel midpoint connecting lines, θ represents the heading angle of the vehicle, Δs represents the equivalent driven shaft speed, Δθ represents the heading angle variation, Δw cl Indicating the displacement of the left driven wheel relative to the previous moment, deltaW cr Representing the displacement of the right driven wheel of the current moment relative to the previous moment, s W Indicating the current steering wheel angle, i S Representing the rotation angle ratio, W B Representing the front-rear wheel separation.
Optionally, the judging unit includes:
the first judging module is used for judging whether a preset conflict determination overlapping area exists in the grid map or not in two ultrasonic measured values in the plurality of ultrasonic measured values;
the second judging module is used for judging whether the barrier measurement results of the overlapping areas determined by the two ultrasonic measurement values aiming at the preset conflict are the same or not if the overlapping areas determined by the preset conflict exist;
And the conflict determination module is used for determining that the two ultrasonic measurement values are in conflict if the two ultrasonic measurement values are different.
Optionally, the map updating unit includes:
the grid updating module is used for marking the same grid to be updated in the historical environment map as an obstacle if all the target ultrasonic measured values covering the grid to be updated determine that the obstacle exists at the grid to be updated; if all the target ultrasonic measurement values covering the grid to be updated do not determine that the obstacle exists at the grid to be updated, marking the grid to be updated as the obstacle does not exist; the grid to be updated comprises a grid which is covered by a target ultrasonic measured value in grids which are not detected by an obstacle in the historical environment map;
and the pose updating module is used for updating the pose of the vehicle in the historical environment map according to the current pose information.
Optionally, the map updating unit further includes:
and the area adjusting module is used for expanding the edge area in front of the vehicle in the historical environment map according to the current pose information and deleting the edge area behind the vehicle so as to ensure that the size of the updated historical environment map is unchanged.
Optionally, the apparatus further includes:
and the storage unit is used for storing each grid in the updated historical environment map in a three-value form, wherein the three values comprise obstacles, no obstacles and no ultrasonic detection.
In a third aspect, embodiments of the present invention provide a storage medium having stored thereon executable instructions which when executed by a processor cause the processor to implement the method of the first aspect.
In a fourth aspect, an embodiment of the present invention provides an autonomous vehicle, including:
one or more processors;
storage means for storing one or more programs,
wherein the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of the first aspect.
As can be seen from the foregoing, the method and apparatus for generating an environment map based on ultrasound according to the embodiments of the present invention can determine whether there is a conflict in a plurality of ultrasound measurement values, and when there is a conflict, remove invalid (i.e., low-confidence) ultrasound measurement values by using the confidence of the conflicting ultrasound measurement values, so as to obtain valid ultrasound measurement values (i.e., target ultrasound measurement values) that do not conflict with each other, and finally update the historical environment map by using the valid ultrasound measurement values and the current pose information of the vehicle, thereby avoiding affecting the accuracy of the environment map due to interference of a plurality of ultrasound signals. Of course, it is not necessary for any one product or method of practicing the invention to achieve all of the advantages set forth above at the same time.
The technical effects of the embodiment of the invention are as follows:
1. by evaluating the confidence coefficient of the ultrasonic measurement value with the conflict, and removing the ultrasonic measurement value with the minimum confidence coefficient until the effective ultrasonic measurement value without the conflict is obtained, the historical environment map is updated according to the effective ultrasonic measurement value and the current pose information of the vehicle, so that the problem of low accuracy of generating the environment map due to the interference of a plurality of ultrasonic signals is avoided, and the accuracy of the environment map is further improved.
2. When evaluating the confidence coefficient of the ultrasonic measurement value, each ultrasonic measurement value in the conflict sequence can be initialized first, then the confidence coefficient of the current ultrasonic measurement value is updated in an iteration mode according to the initialized confidence coefficient of the current ultrasonic measurement value, the initialized confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value and an iteration formula until the change amount of the confidence coefficient of each ultrasonic measurement value in the conflict sequence is smaller than a preset difference threshold value, the iteration reaches convergence, and the final confidence coefficient of each ultrasonic measurement value is obtained.
3. When the current pose information of the vehicle is calculated, the current pose information of the vehicle can be accurately calculated according to the pose information of the vehicle at the moment and the running state of the vehicle (comprising the current steering wheel rotation angle, the left driven wheel displacement, the right driven wheel displacement, the front-rear wheel distance and the rotation angle ratio).
4. When the historical environment map is updated, the current grid is marked as the obstacle only when the obstacle exists at the current grid and the obstacle does not exist at the current grid in the historical environment map is determined by covering all the target ultrasonic measured values of the current grid, so that the accuracy of marking the obstacle is ensured, and the accuracy of updating the historical environment map is further improved.
5. When the historical environment map is updated, in order to reduce memory occupation and ensure feasibility of path planning according to the environment map, the size of the environment map can be kept within a preset range with a vehicle as a center, when the vehicle runs forwards, the edge area in front of the vehicle in the environment map is expanded, and meanwhile, the edge area behind the vehicle is deleted, so that the environment map is kept unchanged in size.
6. By storing each grid in the environment map in a three-value form, the data volume of the environment map can be reduced, thereby saving storage space.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below. It is apparent that the drawings in the following description are only some embodiments of the invention. Other figures may be derived from these figures without inventive effort for a person of ordinary skill in the art.
FIG. 1 is a flowchart of an environment map generating method based on ultrasonic waves provided by an embodiment of the invention;
FIG. 2 is a schematic diagram of a grid map of ultrasonic measurements according to an embodiment of the present invention;
FIG. 3 is a schematic view of a grid map of another ultrasound measurement provided by an embodiment of the present invention;
FIG. 4 is a schematic diagram of an overlapping relationship of ultrasonic measurement values according to an embodiment of the present invention;
FIG. 5 is an expanded schematic view of an environment map according to an embodiment of the present invention;
fig. 6 is a block diagram of an environment map generating device based on ultrasonic waves according to an embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. It will be apparent that the described embodiments are only some, but not all, embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without any inventive effort, are intended to be within the scope of the invention.
It should be noted that the terms "comprising" and "having" and any variations thereof in the embodiments of the present invention and the accompanying drawings are intended to cover non-exclusive inclusions. A process, method, system, article, or apparatus that comprises a list of steps or elements is not limited to only those listed but may alternatively include other steps or elements not listed or inherent to such process, method, article, or apparatus.
The invention provides an environment map generation method and device based on ultrasonic waves, which can acquire an effective ultrasonic measurement value without conflict by removing the ultrasonic measurement value with low confidence coefficient, and update a historical environment map by the effective ultrasonic measurement value, thereby improving the accuracy of generating the environment map. The method provided by the embodiment of the invention can be applied to any electronic equipment with computing capability, and the electronic equipment can be a terminal or a server. In one implementation, the functional software implementing the method may exist in the form of separate client software or may exist in the form of a plug-in to the currently relevant client software.
The following describes embodiments of the present invention in detail.
Fig. 1 is a schematic flow chart of an environment map generating method based on ultrasonic waves according to an embodiment of the present invention. The method may comprise the steps of:
s100: judging whether the ultrasonic measured values have conflict or not according to obstacle measured results represented by the ultrasonic measured values in a grid map; if there is a conflict, step S110 is performed; if there is no conflict, step S140 is performed.
The ultrasonic sensor mounted on the vehicle may detect an obstacle in real time or periodically, and after acquiring an ultrasonic measurement signal, the ultrasonic measurement signal may be analyzed to map a measurement result of whether the obstacle is included on a grid map. As shown in fig. 2, there are 2 ultrasonic measurement values, based on the ultrasonic measurement principle, an area covered by one ultrasonic measurement value is a sector-shaped area, a sector-shaped area in the area is a measurement target, if a certain grid in the sector-shaped area has an obstacle, the grid can be distinguished from other grids, for example, a grid with an obstacle can be represented by black, a grid without an obstacle can be represented by white (as in the other area detected by ultrasonic measurement value 1 in fig. 2), and an area uncovered by the ultrasonic measurement value is represented by gray.
After a plurality of ultrasonic measurement values are obtained, for any two ultrasonic measurement values in the plurality of ultrasonic measurement values, whether a preset conflict determination overlapping area exists in the grid map or not is judged; if the preset conflict determination overlapping area exists, judging whether barrier measurement results of the two ultrasonic measurement values in the preset conflict determination overlapping area are the same or not; if the two ultrasonic measurement values are different, determining that the two ultrasonic measurement values have conflict; and if the preset conflict determination overlapping area does not exist, or the preset conflict determination overlapping area exists and the obstacle measurement results of the two ultrasonic measurement values of the preset conflict determination overlapping area are the same, determining that the two ultrasonic measurement values do not conflict. The specific shape of the preset conflict determination overlapping area may be a sector ring shape, or may be another shape, and may be specifically set according to factors such as a shape and an accuracy requirement defined by ultrasonic detection, which is not limited in the embodiment of the present disclosure. For example, the preset conflict determination overlapping area includes an overlapping sector-shaped area, which may be a sector-shaped area as shown in fig. 2, and if only a partial area of a certain sector-shaped area of two ultrasonic measurement values overlaps, the overlapping area is not the preset conflict determination overlapping area in the embodiment of the present invention, so that there is no conflict in this case.
For example, as shown in fig. 2, the dashed area is an overlapping area determined for the preset conflict of the ultrasonic measurement values 1 and 2, each grid in the dashed area is traversed, whether the measurement results of the two ultrasonic measurement values on the grid are the same is determined, the final determination result is that all grids in the dashed area are determined to be free of obstacles in the ultrasonic measurement value 1, while at least one obstacle is considered to exist in the area in the ultrasonic measurement value 2, and at least one measurement error is necessarily caused in the two measurements, and the conflict of the ultrasonic measurement values 1 and 2 is defined. In fig. 3, the ultrasonic measurement value 3 (the upper sector in the drawing is ultrasonic measurement value 3) is a sector-shaped region that only partially covers the ultrasonic measurement value 4 (the lower sector in the drawing is ultrasonic measurement value 3), so that it is determined that there is no collision between the ultrasonic measurement values 3 and 4.
It should be noted that, in the embodiment of the present invention, there may be one or a plurality of ultrasonic sensors. When the ultrasonic sensor is one, the plurality of ultrasonic measured values are measured values of the ultrasonic sensor at different moments; when there are a plurality of ultrasonic sensors, the ultrasonic measurement values include measurement values of the same ultrasonic sensor at different times and/or measurement values of different ultrasonic sensors at the same time. When the ultrasonic sensor is installed at different positions, the ultrasonic measured values at different moments and different positions can be used for generating an environment map, so that errors and dead zones existing in single measurement are eliminated, and the whole scheme does not need to rely on positioning equipment additionally. For each ultrasonic measurement value obtained by the current ultrasonic measurement technology, the coverage of the ultrasonic measurement value can be a sector range, so that only whether an obstacle exists in the measurement distance corresponding to the sector range can be known, but the specific size of the obstacle, the position of the finer granularity can be further determined by other non-conflicting ultrasonic measurement values.
In addition, the embodiment of the invention can project all ultrasonic measured values on the same grid map, is insensitive to the change of the number and the position arrangement of the ultrasonic sensors, and can be suitable for different vehicle types.
S110: at least two ultrasound measurements for which a conflict exists are formed into a conflict sequence, and the confidence of each ultrasound measurement in the conflict sequence is initialized.
After a collision determination is made for any two of the plurality of ultrasonic measurements, at least two ultrasonic measurements that are in collision may be formed into a collision sequence, and each ultrasonic measurement in the collision sequence is in collision with at least one other ultrasonic measurement in the collision sequence, rather than having to cause any two ultrasonic measurements in the collision sequence to be in collision. Furthermore, there may be one conflicting sequence for a plurality of ultrasound measurements, or there may be a plurality of conflicting sequences. For example, if there is a collision between ultrasonic measurement 1 and ultrasonic measurement 2, and if there is a collision between ultrasonic measurement 2 and ultrasonic measurement 1, 3, and ultrasonic measurement 4 has a collision between ultrasonic measurement 5 and ultrasonic measurement 6, respectively, then ultrasonic measurement 1-3 may be configured into collision sequence 1, and ultrasonic measurement 4-6 may be configured into collision sequence 2.
After obtaining at least one collision sequence, the confidence of each ultrasonic measurement in each collision sequence may be initialized to obtain an initialized confidence for each ultrasonic measurement for subsequent updating of the confidence for each ultrasonic measurement for each collision sequence. The confidence level of each ultrasonic measurement value may be uniformly initialized to a value, for example, may be 0.5, or may be 0.4, 0.6, or 0.55. If the detection result is known a priori, the initialization confidence level can be appropriately adjusted, for example, if the ultrasonic sensor is known to be inaccurate when the detection distance is far, the initialization confidence level of the measured value of the ultrasonic sensor can be lower than that of other measured values.
S120: and updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value, so as to obtain the target confidence coefficient of the current ultrasonic measurement value.
Specifically, the confidence level of the current ultrasonic measurement value can be iteratively updated according to the initialization confidence level of the current ultrasonic measurement value in the conflict sequence and the initialization confidence level of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value; and when the difference between the confidence coefficient of each ultrasonic measurement value after the latest two iterative updating in the conflict sequence is smaller than a preset difference threshold value, determining the confidence coefficient of the current ultrasonic measurement value after the latest iterative updating as the target confidence coefficient.
The iterative updating process specifically comprises the following steps: substituting the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value into an iterative formula, and carrying out iterative updating on the confidence coefficient of the current ultrasonic measurement value; the iterative formula is as follows: p (P) k+1 (i)＝(1-Π j∈ S P k (j))+(Π j∈S P k (j))P k (i)；
Where i represents the ith ultrasonic measurement value in the conflict sequence, S represents the set of ultrasonic measurement values in the conflict sequence that conflict with the ith ultrasonic measurement value, which may be called S sequence, j represents the jth ultrasonic measurement value in the set of ultrasonic measurement values, P represents the confidence level, the confidence level when k=1 is the initialization confidence level, and k is increased by 1 each iteration.
For example, as shown in FIG. 4, a collision sequence includes three ultrasonic measurements, where ultrasonic measurement 1 collides with ultrasonic measurement 2, ultrasonic measurement 2 collides with ultrasonic measurement 1, 3, and ultrasonic measurement 3 collides with ultrasonic measurement 1, then when the confidence of ultrasonic measurement 1 is calculated iteratively, the corresponding S sequence is {2}, when iterating When the confidence of the ultrasonic measurement value 2 is calculated, the corresponding S sequence is {1,3}, and when the confidence of the ultrasonic measurement value 3 is calculated iteratively, the corresponding S sequence is {2}. The initialization confidence of the ultrasonic measurement values 1, 2 and 3 are respectively P 1 (1)、P 1 (2)、P 1 (3)。
k=1:
confidence level P of ultrasonic measurement 1 2 (1)＝(1-P 1 (2))+P 1 (2)P 1 (1)；
Confidence P of ultrasonic measurement 2 2 (2)＝(1-P 1 (1)P 1 (3))+P 1 (1)P 1 (3)P 1 (2)；
Confidence P of ultrasonic measurement 3 2 (3)＝(1-P 1 (2))+P 1 (2)P 1 (3)。
k=2:
confidence level P of ultrasonic measurement 1 3 (1)＝(1-P 2 (2))+P 2 (2)P 2 (1)；
Confidence P of ultrasonic measurement 2 3 (2)＝(1-P 2 (1)P 2 (3))+P 2 (1)P 2 (3)P 2 (2)；
Confidence P of ultrasonic measurement 3 3 (3)＝(1-P 2 (2))+P 2 (2)P 2 (3)。
k=3:
confidence level P of ultrasonic measurement 1 4 (1)＝(1-P 3 (2))+P 3 (2)P 3 (1)；
Confidence P of ultrasonic measurement 2 4 (2)＝(1-P 3 (1)P 3 (3))+P 3 (1)P 3 (3)P 3 (2)；
Confidence P of ultrasonic measurement 3 4 (3)＝(1-P 3 (2))+P 3 (2)P 3 (3)。
If P 4 (1)-P 3 (1)、P 4 (2)-P 3 (2)、P 4 (3)-P 3 (3) If the iteration parameters are smaller than the preset difference threshold value, the iteration can be determined to reach convergence, and P can be determined to be equal to 4 (1)、P 4 (2)、P 4 (3) As a target confidence.
S130: and removing the ultrasonic measured value with the minimum target confidence coefficient in the conflict sequence. The process returns to step S100.
Since the smaller the confidence level is, the larger the error of the ultrasonic measurement value is, the ultrasonic measurement value with the minimum target confidence level in the collision sequence can be removed, the remaining ultrasonic measurement values in the collision sequence and the ultrasonic measurement values not listed in the collision sequence are used as the current remaining ultrasonic measurement values, and the steps S100-S130 are continuously executed until the current remaining ultrasonic measurement values have no collision, and the current remaining ultrasonic measurement values are determined to be the target ultrasonic measurement values.
S140: the current remaining ultrasonic measurement value is determined as the target ultrasonic measurement value.
S150: and updating the historical environment map according to the current pose information determined by the pose information at the moment on the vehicle and the current running state of the vehicle and the target ultrasonic measured value.
The current running state of the vehicle can be described by parameters such as the current steering wheel rotation angle of the vehicle, the displacement of a left driven wheel at the current moment relative to the last moment, the displacement of a right driven wheel at the current moment relative to the last moment, the wheelbase of the front wheel and the rear wheel, the rotation angle ratio of the front wheel and the steering wheel at the current moment and the like. Pose information includes the geographic position and pose (also referred to as pose direction) of the vehicle. Because the vehicle body size is larger, a certain point on the vehicle can be taken to represent the geographic position of the vehicle in practical application, for example, the midpoint of the connecting line of the midpoint of the two rear wheels can be taken as the geographic position of the vehicle; similarly, because the vehicle body is larger, in practical application, when the directions of the vehicle heads are different for the same geographic position, the vehicle presents different postures, and the specific implementation can use the course angle to represent the posture of the vehicle. Thus, the method for determining the current pose information specifically may be: acquiring pose information of the previous moment, a current steering wheel rotation angle of the vehicle, left driven wheel displacement of the current moment relative to the previous moment, right driven wheel displacement of the current moment relative to the previous moment, front and rear wheel wheelbases and a rotation angle ratio of a front wheel and a steering wheel at the current moment; and determining the current pose information according to the pose information at the last moment, the current steering wheel corner, the left driven wheel displacement, the right driven wheel displacement, the front-rear wheel distance and the corner ratio.
Calculating the current pose information by the following formula:
wherein,representing the current pose information, +.>Pose information representing the last moment,
x represents the abscissa of the midpoint of the two rear wheel midpoint connecting lines, y represents the ordinate of the midpoint of the two rear wheel midpoint connecting lines, θ represents the heading angle of the vehicle, Δs represents the equivalent driven shaft speed, Δθ represents the heading angle variation, Δw cl Indicating the displacement of the left driven wheel relative to the previous moment, deltaW cr Representing the displacement of the right driven wheel of the current moment relative to the previous moment, s W Indicating the current steering wheel angle, i S Representing the rotation angle ratio, W B Representing the front-rear wheel separation. The front-rear wheel axle distance is the distance from the center of the front wheel axle to the center of the rear wheel axle on the same side when the vehicle is in an initial state (i.e. no wheel turning or the like exists).
After the current pose information of the vehicle and the target ultrasonic measurement value are obtained, each grid in the history environment map can be traversed, and if the current traversed grid is a grid (such as an area indicated by S3-1 in FIG. 3) which is detected by an obstacle in the history environment map or a grid (such as an area indicated by S3-3 in FIG. 3) which is not detected by the obtained target ultrasonic measurement value at this time, the original information is reserved; otherwise, for the same grid to be updated, if all the target ultrasonic measurement values covering the grid to be updated determine that an obstacle exists at the grid to be updated (such as the area indicated in S3-2 in fig. 3), and the historical environment map does not determine that no obstacle exists at the grid to be updated, marking the grid to be updated as an obstacle exists; if all the target ultrasonic measurement values covering the grid to be updated do not determine that an obstacle exists at the grid to be updated (such as a region indicated by S3-4 in FIG. 3), marking the grid to be updated as not having the obstacle; the grid to be updated comprises a grid which is covered by a target ultrasonic measured value in grids which are not detected by the obstacle in the historical environment map. In addition, the pose of the vehicle needs to be updated in the historical environment map according to the current pose information.
In one embodiment, when updating the historical environment map, in order to reduce memory occupation and ensure feasibility of path planning according to the environment map, the size of the environment map can be kept within a preset range centered on the vehicle, when the vehicle travels forwards, an edge area in front of the vehicle in the environment map is expanded, and an edge area behind the vehicle is deleted, so that the environment map is kept unchanged in size. For example, as shown in fig. 5, when the vehicle travels forward one grid, the right side edge of the history environment map may be expanded by one grid, and the left side edge may be deleted by one grid, so that the size of the environment map remains unchanged.
In one embodiment, to save storage space, the data volume of the environment map may be reduced by storing each grid in the updated historical environment map in a three-valued form. The three values include obstacles, no obstacles, and no ultrasonic detection. For example, there are 16 grids in the environment map, with, without and without ultrasonic detection represented by 0, 1 and 2, respectively, and then the environment map can be represented by a matrix.
For example as a matrix
According to the method for generating the environment map based on the ultrasonic wave, after the plurality of ultrasonic wave measured values are obtained, the historical environment map is not updated directly according to the plurality of ultrasonic wave measured values, whether the plurality of ultrasonic wave measured values collide or not is judged first, when the plurality of ultrasonic wave measured values collide, invalid (namely, low-confidence) ultrasonic wave measured values are removed by means of confidence of the colliding ultrasonic wave measured values, so that effective ultrasonic wave measured values (namely, target ultrasonic wave measured values) which do not collide with each other are obtained, and finally, the historical environment map is updated by means of the effective ultrasonic wave measured values and current pose information of a vehicle, so that influence on the accuracy of the environment map due to interference of a plurality of ultrasonic wave signals is avoided.
Corresponding to the above method embodiment, the embodiment of the present invention provides an environment map generating device based on ultrasonic, as shown in fig. 6, the device may include:
a judging unit 60 for judging whether or not there is a conflict in a plurality of ultrasonic measurement values based on obstacle measurement results characterized in a grid map by the plurality of ultrasonic measurement values;
a generating unit 62, configured to, when there is a conflict among the plurality of ultrasonic measurement values, configure at least two ultrasonic measurement values that have a conflict into a conflict sequence, where each ultrasonic measurement value in the conflict sequence has a conflict with at least one other ultrasonic measurement value in the conflict sequence;
An initializing unit 64 for initializing a confidence level of each ultrasonic measurement value in the collision sequence;
a confidence updating unit 66, configured to update the confidence level of the current ultrasonic measurement value according to the initialization confidence level of the current ultrasonic measurement value in the collision sequence and the initialization confidence level of the ultrasonic measurement value that has a collision with the current ultrasonic measurement value, so as to obtain a target confidence level of the current ultrasonic measurement value;
a removing unit 68, configured to remove the ultrasonic measurement value with the minimum target confidence coefficient in the collision sequence; returning to the judging unit executing step, judging whether the plurality of ultrasonic measured values have conflict or not according to obstacle measured results represented by the plurality of ultrasonic measured values in the grid map;
a determining unit 610 configured to determine, when there is no conflict in the currently remaining ultrasonic measurement values, the currently remaining ultrasonic measurement values as target ultrasonic measurement values;
a map updating unit 612 for updating the historical environment map according to the current pose information determined by the pose information of the vehicle at the previous moment and the current running state of the vehicle and the target ultrasonic measurement value.
Optionally, the confidence updating unit 66 includes:
the iteration updating module is used for carrying out iteration updating on the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value;
and the confidence coefficient determining module is used for determining the confidence coefficient of the current ultrasonic measurement value after the last iteration update as the target confidence coefficient when the difference between the confidence coefficient of each ultrasonic measurement value after the last iteration update of the two times in the conflict sequence is smaller than a preset difference threshold value.
Optionally, the iteration updating module is configured to substitute the initialization confidence coefficient of the current ultrasonic measurement value in the collision sequence and the initialization confidence coefficient of the ultrasonic measurement value that collides with the current ultrasonic measurement value into an iteration formula, and iteratively update the confidence coefficient of the current ultrasonic measurement value;
the iterative formula is as follows: p (P) k+1 (i)＝(1-Π j∈S P k (j))+(Π j∈S P k (j))P k (i)，
Wherein i represents an ith ultrasonic measurement value in the conflict sequence, S represents an ultrasonic measurement value set in the conflict sequence, which conflicts with the ith ultrasonic measurement value, j represents a jth ultrasonic measurement value in the ultrasonic measurement value set, P represents a confidence level, the confidence level when k=1 is an initialization confidence level, and k is increased by 1 every iteration.
Optionally, the device further comprises a pose determining unit, which is used for determining current pose information according to pose information of the vehicle at the moment and the current running state of the vehicle;
the pose determination unit includes:
the acquisition module is used for acquiring pose information of the previous moment, the current steering wheel rotation angle of the vehicle, left driven wheel displacement of the current moment relative to the previous moment, right driven wheel displacement of the current moment relative to the previous moment, wheel base of front wheels and rear wheels and the rotation angle ratio of the front wheels and the steering wheel at the current moment;
and the pose determining module is used for determining the current pose information according to the pose information at the last moment, the current steering wheel rotation angle, the left driven wheel displacement, the right driven wheel displacement, the front and rear wheel distances and the rotation angle ratio.
Optionally, the pose determining module is configured to calculate the current pose information according to the following formula:
/>
wherein,representing the current pose information, +.>Pose information representing the last moment,
x represents the abscissa of the midpoint of the two rear wheel midpoint connecting lines, y represents the ordinate of the midpoint of the two rear wheel midpoint connecting lines, θ represents the heading angle of the vehicle, Δs represents the equivalent driven shaft speed, Δθ represents the heading angle variation, Δw cl Indicating the displacement of the left driven wheel relative to the previous moment, deltaW cr Representing the displacement of the right driven wheel of the current moment relative to the previous moment, s W Indicating the current steering wheel angle, i S Representing the rotation angle ratio, W B Representing the front-rear wheel separation.
Optionally, the judging unit 60 includes:
the first judging module is used for judging whether a preset conflict determination overlapping area exists in the grid map or not in two ultrasonic measured values in the plurality of ultrasonic measured values;
the second judging module is used for judging whether the barrier measurement results of the overlapping areas determined by the two ultrasonic measurement values aiming at the preset conflict are the same or not if the overlapping areas determined by the preset conflict exist;
and the conflict determination module is used for determining that the two ultrasonic measurement values are in conflict if the two ultrasonic measurement values are different.
Optionally, the map updating unit 612 includes:
the grid updating module is used for marking the grid to be updated as an obstacle if all the target ultrasonic measured values covering the grid to be updated determine that the obstacle exists at the grid to be updated, marking the grid to be updated as an absence of the obstacle if all the target ultrasonic measured values covering the grid to be updated do not determine that the obstacle exists at the grid to be updated, wherein the grid to be updated comprises a grid covered by the target ultrasonic measured values which are not detected by the obstacle in the historical environment map;
And the pose updating module is used for updating the pose of the vehicle in the historical environment map according to the current pose information.
Optionally, the map updating unit 612 further includes:
and the area adjusting module is used for expanding the edge area in front of the vehicle in the historical environment map according to the current pose information and deleting the edge area behind the vehicle so as to ensure that the size of the updated historical environment map is unchanged.
Optionally, the apparatus further includes:
and the storage unit is used for storing each grid in the updated historical environment map in a three-value form, wherein the three values comprise obstacles, no obstacles and no ultrasonic detection.
According to the ultrasonic-based environment map generation device provided by the embodiment of the invention, after the plurality of ultrasonic measurement values are obtained, the historical environment map is not updated directly according to the plurality of ultrasonic measurement values, whether the plurality of ultrasonic measurement values collide or not is judged first, when the plurality of ultrasonic measurement values collide, invalid (namely, low-confidence) ultrasonic measurement values are removed by using the confidence coefficient of the colliding ultrasonic measurement values so as to obtain effective ultrasonic measurement values (namely, target ultrasonic measurement values) which do not collide with each other, and finally, the historical environment map is updated by using the effective ultrasonic measurement values and the current pose information of the vehicle, so that the influence on the accuracy of the environment map due to interference of a plurality of ultrasonic signals is avoided.
Based on the above method embodiments, another embodiment of the present invention provides a storage medium having stored thereon executable instructions that, when executed by a processor, cause the processor to implement the method as described above.
Based on the above method embodiments, another embodiment of the present invention provides an autonomous vehicle, including:
one or more processors;
storage means for storing one or more programs,
wherein the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the methods as described above.
The system and device embodiments correspond to the method embodiments, and have the same technical effects as the method embodiments, and specific description refers to the method embodiments. The apparatus embodiments are based on the method embodiments, and specific descriptions may be referred to in the method embodiment section, which is not repeated herein. Those of ordinary skill in the art will appreciate that: the drawing is a schematic diagram of one embodiment and the modules or flows in the drawing are not necessarily required to practice the invention.
Those of ordinary skill in the art will appreciate that: the modules in the apparatus of the embodiments may be distributed in the apparatus of the embodiments according to the description of the embodiments, or may be located in one or more apparatuses different from the present embodiments with corresponding changes. The modules of the above embodiments may be combined into one module, or may be further split into a plurality of sub-modules.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some of the technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims (7)
1. An ultrasound-based environmental map generation method, the method comprising:
judging whether the ultrasonic measured values have conflict or not according to obstacle measured results represented by the ultrasonic measured values in a grid map;
when the plurality of ultrasonic measurement values conflict, forming a conflict sequence by at least two ultrasonic measurement values with conflicts, and initializing the confidence of each ultrasonic measurement value in the conflict sequence, wherein each ultrasonic measurement value in the conflict sequence conflicts with at least one other ultrasonic measurement value in the conflict sequence;
Updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value to obtain the target confidence coefficient of the current ultrasonic measurement value;
removing the ultrasonic measurement value with the minimum target confidence coefficient in the conflict sequence, returning to the execution step, judging whether the plurality of ultrasonic measurement values conflict or not according to the obstacle measurement results represented by the plurality of ultrasonic measurement values in the grid map, and determining the current remaining ultrasonic measurement values as target ultrasonic measurement values when no conflict exists in the current remaining ultrasonic measurement values;
updating a historical environment map according to current pose information determined by pose information of the vehicle at the moment and the current running state of the vehicle and the target ultrasonic measurement value;
judging whether the plurality of ultrasonic measurement values have conflict according to obstacle measurement results characterized by the plurality of ultrasonic measurement values in a grid map, comprising: judging whether two ultrasonic measured values in the plurality of ultrasonic measured values have preset conflict in the grid map to determine an overlapping area; if the preset conflict determination overlapping area exists, judging whether barrier measurement results of the two ultrasonic measurement values in the preset conflict determination overlapping area are the same or not; if the two ultrasonic measurement values are different, determining that the two ultrasonic measurement values have conflict;
Updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value, and obtaining the target confidence coefficient of the current ultrasonic measurement value, wherein the method comprises the following steps: iteratively updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value; when the difference between the confidence coefficient of each ultrasonic measurement value after the latest two iterative updating in the conflict sequence is smaller than a preset difference threshold value, determining the confidence coefficient of the current ultrasonic measurement value after the latest iterative updating as the target confidence coefficient;
updating a historical environment map according to current pose information determined by pose information at the moment of the vehicle and the current running state of the vehicle and the target ultrasonic measured value, comprising: for the same grid to be updated in the historical environment map, if all the target ultrasonic measured values covering the grid to be updated determine that an obstacle exists at the grid to be updated, marking the grid to be updated as the obstacle; if all the target ultrasonic measurement values covering the grid to be updated do not determine that the obstacle exists at the grid to be updated, marking the grid to be updated as the obstacle does not exist; wherein the grid to be updated comprises a grid covered by a target ultrasonic measured value in grids which are not detected by an obstacle in the historical environment map; and updating the pose of the vehicle in the historical environment map according to the current pose information.
2. The method of claim 1, wherein iteratively updating the confidence of the current ultrasound measurement based on the initialization confidence of the current ultrasound measurement in the conflicting sequence, the initialization confidence of the ultrasound measurement that conflicts with the current ultrasound measurement, comprises:
substituting the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value into an iterative formula, and carrying out iterative updating on the confidence coefficient of the current ultrasonic measurement value;
the iterative formula is as follows: p (P) k+1 (i)＝(1-Π j∈S P k (j))+(Π j∈S P k (j))P k (i)，
Wherein i represents an ith ultrasonic measurement value in the conflict sequence, S represents an ultrasonic measurement value set in the conflict sequence, which conflicts with the ith ultrasonic measurement value, j represents a jth ultrasonic measurement value in the ultrasonic measurement value set, P represents a confidence level, the confidence level when k=1 is an initialization confidence level, and k is increased by 1 every iteration.
3. The method of claim 1, wherein determining current pose information from pose information at a time on a vehicle and a current running state of the vehicle comprises:
Acquiring pose information of the previous moment, a current steering wheel rotation angle of the vehicle, left driven wheel displacement of the current moment relative to the previous moment, right driven wheel displacement of the current moment relative to the previous moment, front and rear wheel wheelbases and a rotation angle ratio of a front wheel and a steering wheel at the current moment;
and determining the current pose information according to the pose information at the last moment, the current steering wheel corner, the left driven wheel displacement, the right driven wheel displacement, the front-rear wheel distance and the corner ratio.
4. The method of claim 3, wherein determining the current pose information based on the last moment pose information, the current steering wheel angle, the left driven wheel displacement, the right driven wheel displacement, the front-to-rear wheel spacing, and the angle of rotation ratio comprises:
calculating the current pose information by the following formula:
wherein,representing the current pose information, +.>Pose information representing the last moment,
x represents the abscissa of the midpoint of the two rear wheel midpoint connecting lines, y represents the ordinate of the midpoint of the two rear wheel midpoint connecting lines, θ represents the heading angle of the vehicle, Δs represents the equivalent driven shaft speed, Δθ represents the heading angle variation, Δw cl Indicating the displacement of the left driven wheel relative to the previous moment, deltaW cr Representing the displacement of the right driven wheel of the current moment relative to the previous moment, s W Indicating the current steering wheel angle, i S Representing the rotation angle ratio, W B Representing the front-rear wheel separation.
5. The method of claim 1, wherein updating the historical environment map based on the current pose information determined from the pose information at a time on the vehicle and the current running state of the vehicle and the target ultrasonic measurement value, further comprises:
and expanding the edge area in front of the vehicle in the historical environment map according to the current pose information, and deleting the edge area in back of the vehicle so as to ensure that the size of the updated historical environment map is unchanged.
6. The method of any one of claims 1-5, wherein the method further comprises:
and storing each grid in the updated historical environment map in a three-value form, wherein the three values comprise obstacles, no obstacles and no ultrasonic detection.
7. An ultrasound-based environment map generation apparatus, the apparatus comprising:
A judging unit for judging whether the plurality of ultrasonic measurement values have conflict according to the obstacle measurement results represented by the plurality of ultrasonic measurement values in the grid map;
a generating unit, configured to, when there is a conflict in the plurality of ultrasonic measurement values, configure at least two ultrasonic measurement values that have a conflict into a conflict sequence, where each ultrasonic measurement value in the conflict sequence has a conflict with at least one other ultrasonic measurement value in the conflict sequence;
an initializing unit, configured to initialize a confidence level of each ultrasonic measurement value in the collision sequence;
the confidence coefficient updating unit is used for updating the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value to obtain the target confidence coefficient of the current ultrasonic measurement value;
the removing unit is used for removing the ultrasonic measurement value with the minimum target confidence coefficient in the conflict sequence, returning to the judging unit, and executing the step of judging whether the plurality of ultrasonic measurement values conflict or not according to the obstacle measurement results represented by the plurality of ultrasonic measurement values in the grid map;
A determining unit configured to determine, when there is no conflict in the current remaining ultrasonic measurement values, the current remaining ultrasonic measurement values as target ultrasonic measurement values;
a map updating unit for updating a historical environment map according to current pose information determined by pose information at the moment of the vehicle and the current running state of the vehicle and the target ultrasonic measurement value;
the judging unit includes:
the first judging module is used for judging whether a preset conflict determination overlapping area exists in the grid map or not in two ultrasonic measured values in the plurality of ultrasonic measured values;
the second judging module is used for judging whether the barrier measurement results of the overlapping areas determined by the two ultrasonic measurement values aiming at the preset conflict are the same or not if the overlapping areas determined by the preset conflict exist;
the conflict determination module is used for determining that the two ultrasonic measurement values are in conflict if the two ultrasonic measurement values are different;
the confidence updating unit includes:
the iteration updating module is used for carrying out iteration updating on the confidence coefficient of the current ultrasonic measurement value according to the initialization confidence coefficient of the current ultrasonic measurement value in the conflict sequence and the initialization confidence coefficient of the ultrasonic measurement value which conflicts with the current ultrasonic measurement value;
The confidence coefficient determining module is used for determining the confidence coefficient of each ultrasonic measurement value after the latest iteration update of the current ultrasonic measurement value as the target confidence coefficient when the difference between the confidence coefficient of each ultrasonic measurement value in the conflict sequence after the latest iteration update is smaller than a preset difference threshold value;
the map updating unit includes:
the grid updating module is used for marking the same grid to be updated in the historical environment map as an obstacle if all the target ultrasonic measured values covering the grid to be updated determine that the obstacle exists at the grid to be updated; if all the target ultrasonic measurement values covering the grid to be updated do not determine that the obstacle exists at the grid to be updated, marking the grid to be updated as the obstacle does not exist; the grid to be updated comprises a grid which is covered by a target ultrasonic measured value in grids which are not detected by an obstacle in the historical environment map;
and the pose updating module is used for updating the pose of the vehicle in the historical environment map according to the current pose information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110946715.6A CN113670296B (en) | 2021-08-18 | 2021-08-18 | Method and device for generating environment map based on ultrasonic waves |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110946715.6A CN113670296B (en) | 2021-08-18 | 2021-08-18 | Method and device for generating environment map based on ultrasonic waves |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113670296A CN113670296A (en) | 2021-11-19 |
| CN113670296B true CN113670296B (en) | 2023-11-24 |
Family
ID=78543493
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110946715.6A Active CN113670296B (en) | 2021-08-18 | 2021-08-18 | Method and device for generating environment map based on ultrasonic waves |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113670296B (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104916216A (en) * | 2015-06-26 | 2015-09-16 | 深圳乐行天下科技有限公司 | Map construction method and system thereof |
| CN105043396A (en) * | 2015-08-14 | 2015-11-11 | 北京进化者机器人科技有限公司 | Method and system for indoor map self-establishment of mobile robot |
| DE102015213558A1 (en) * | 2015-07-20 | 2017-01-26 | Bayerische Motoren Werke Aktiengesellschaft | Device and method for merging two obstacle cards for environment detection |
| CN106996793A (en) * | 2015-11-04 | 2017-08-01 | 丰田自动车株式会社 | Map rejuvenation decision-making system |
| CN107462892A (en) * | 2017-07-28 | 2017-12-12 | 深圳普思英察科技有限公司 | Mobile robot synchronous superposition method based on more sonacs |
| CN109814112A (en) * | 2019-01-15 | 2019-05-28 | 北京百度网讯科技有限公司 | A kind of ultrasonic radar and laser radar information fusion method and system |
| CN111948672A (en) * | 2019-05-17 | 2020-11-17 | 福特全球技术公司 | Belief graph construction using shared data |
| CN112393737A (en) * | 2019-08-16 | 2021-02-23 | 苏州科瓴精密机械科技有限公司 | Obstacle map creation method, obstacle map creation system, robot, and readable storage medium |
| CN112406707A (en) * | 2020-11-24 | 2021-02-26 | 上海高德威智能交通系统有限公司 | Vehicle early warning method, vehicle, device, terminal and storage medium |
-
2021
- 2021-08-18 CN CN202110946715.6A patent/CN113670296B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104916216A (en) * | 2015-06-26 | 2015-09-16 | 深圳乐行天下科技有限公司 | Map construction method and system thereof |
| DE102015213558A1 (en) * | 2015-07-20 | 2017-01-26 | Bayerische Motoren Werke Aktiengesellschaft | Device and method for merging two obstacle cards for environment detection |
| CN105043396A (en) * | 2015-08-14 | 2015-11-11 | 北京进化者机器人科技有限公司 | Method and system for indoor map self-establishment of mobile robot |
| CN106996793A (en) * | 2015-11-04 | 2017-08-01 | 丰田自动车株式会社 | Map rejuvenation decision-making system |
| CN107462892A (en) * | 2017-07-28 | 2017-12-12 | 深圳普思英察科技有限公司 | Mobile robot synchronous superposition method based on more sonacs |
| CN109814112A (en) * | 2019-01-15 | 2019-05-28 | 北京百度网讯科技有限公司 | A kind of ultrasonic radar and laser radar information fusion method and system |
| CN111948672A (en) * | 2019-05-17 | 2020-11-17 | 福特全球技术公司 | Belief graph construction using shared data |
| CN112393737A (en) * | 2019-08-16 | 2021-02-23 | 苏州科瓴精密机械科技有限公司 | Obstacle map creation method, obstacle map creation system, robot, and readable storage medium |
| CN112406707A (en) * | 2020-11-24 | 2021-02-26 | 上海高德威智能交通系统有限公司 | Vehicle early warning method, vehicle, device, terminal and storage medium |
Non-Patent Citations (2)
| Title |
|---|
| 基于声纳传感器的移动机器人地图创建方法研究;房芳;马旭东;戴先中;;计算机应用研究(第07期);全文 * |
| 基于栅格地图的智能车辆运动目标检测;周俊静;段建民;;系统工程与电子技术(第02期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113670296A (en) | 2021-11-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2022022694A1 (en) | Method and system for sensing automated driving environment | |
| US11628857B2 (en) | Correcting a position of a vehicle with SLAM | |
| CN109212531B (en) | Method for determining the orientation of a target vehicle | |
| CN110867132B (en) | Environment sensing method, device, electronic equipment and computer readable storage medium | |
| US11188089B2 (en) | Localization for autonomous vehicles using gaussian mixture models | |
| CN110082747B (en) | Method for increasing reliability of determining vehicle position based on multiple detection points | |
| CN107632308B (en) | Method for detecting contour of obstacle in front of vehicle based on recursive superposition algorithm | |
| US20090040095A1 (en) | Apparatus for estimating state of vehicle located in frontward field | |
| CN109635816B (en) | Lane line generation method, apparatus, device, and storage medium | |
| US11300415B2 (en) | Host vehicle position estimation device | |
| CN110794406B (en) | Multi-source sensor data fusion system and method | |
| US11731649B2 (en) | High precision position estimation method through road shape classification-based map matching and autonomous vehicle thereof | |
| US10802122B1 (en) | Methods and systems for calibration of multiple lidar devices with non-overlapping fields of view | |
| JP7152355B2 (en) | Obstacle detection device and obstacle detection method | |
| CN115792894A (en) | Multi-sensor fusion vehicle target tracking method, system and storage medium | |
| WO2007046336A1 (en) | Object recognizing device | |
| US20230094836A1 (en) | Method for Detecting Moving Objects in the Surroundings of a Vehicle, and Motor Vehicle | |
| CN112581610B (en) | Robust optimization method and system for building map from multi-beam sonar data | |
| CN113670296B (en) | Method and device for generating environment map based on ultrasonic waves | |
| CN113434788A (en) | Mapping method and device, electronic equipment and vehicle | |
| CN114740448B (en) | Target state estimation method and device for vehicle-mounted radar and storage medium | |
| KR20200107382A (en) | Apparatus and method for determining correction information of vehicle-sensor | |
| EP3819665B1 (en) | Method and computer device for calibrating lidar system | |
| WO2022195675A1 (en) | Information processing device, control method, program, and storage medium | |
| JP2001215274A (en) | Vehicle periphery monitoring device |
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 |