CN110346814B - Obstacle detection and obstacle avoidance control method and system based on 3D laser - Google Patents
Obstacle detection and obstacle avoidance control method and system based on 3D laser Download PDFInfo
- Publication number
- CN110346814B CN110346814B CN201810306723.2A CN201810306723A CN110346814B CN 110346814 B CN110346814 B CN 110346814B CN 201810306723 A CN201810306723 A CN 201810306723A CN 110346814 B CN110346814 B CN 110346814B
- Authority
- CN
- China
- Prior art keywords
- point
- vehicle
- obstacle
- original
- path
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Traffic Control Systems (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The invention relates to a 3D laser-based obstacle detection and obstacle avoidance control method and system, comprising the following steps: clustering laser beams on the same plane; fusing the planes to obtain the position information and the range information of the obstacle with the boundary distance larger than the threshold distance; screening out the boundary point closest to the original planned path; reserving the width through which the vehicle can pass to obtain a reference point; determining a target point of vehicle control; triggering obstacle avoidance measures of the vehicle, and controlling the vehicle to move towards the target point. The obstacle avoidance control measure provided by the invention is based on the edge information of the obstacle and the original planned path, the vehicle cannot swing back and forth due to the narrow place, and the vehicle can always travel in the path closest to the original planned path and cannot deviate from the path. In addition, the 3D laser radar used for multiplexing and positioning can be used, and the obtained information of the obstacle is accurate and rich.
Description
Technical Field
The invention relates to an obstacle detection and obstacle avoidance control method and system based on 3D laser, in particular to obstacle avoidance control measures adopted after obstacle detection and obstacle detection in an automatic navigation process.
Background
For the current control navigation system, obstacle detection and obstacle avoidance measures become more and more important, and effective obstacle detection and obstacle avoidance measures not only can improve the safety of vehicle control, but also can increase the efficiency of vehicle control, so that the vehicle can reach a destination efficiently or on time.
For detecting obstacles, sensors such as ultrasonic sensors, photoelectric sensors, vision sensors, laser radars and the like are mainly adopted at present for detection. Ultrasonic and photoelectric sensors can only detect single points and cannot obtain accurate information of obstacles; the vision sensor can obtain information of a large number of obstacles, but the precision is generally not high; the 2D laser radar can obtain accurate obstacle information, but only detects a single plane, and the judgment of the edges of obstacles inconsistent in the up-down direction often has larger error, so that obstacle avoidance control is caused. The obstacle information obtained by the existing obstacle detection mode is low in accuracy or information quantity, and optimal obstacle avoidance control measures are difficult to give according to the information.
Most of the obstacle avoidance measures at present mainly stop the obstacle, namely, as long as the obstacle is detected within a certain distance range of the vehicle, the vehicle is controlled to stop, and the vehicle resumes operation after the obstacle is removed. This approach can seriously affect the efficiency of navigating the vehicle if the obstacle is not removed for a long time. And the obstacle avoidance measures which are stopped when encountering obstacles are adopted, so that the efficiency is low, and the safety protection device can only be used for safety protection. Still another part of obstacle avoidance measures is to use an artificial potential field method, namely, each obstacle is used as a force applied to the vehicle, so that the vehicle moves along the reverse direction of the resultant force of all forces, and obviously, although the method is simple, the vehicle can easily swing back and forth in a narrow channel, and can easily deviate from the original planned path, so that the normal running of the vehicle is affected.
Disclosure of Invention
In view of the above-mentioned drawbacks of the prior art, the present invention detects obstacles based on 3D laser and adopts corresponding obstacle avoidance control. The 3D laser includes all the advantages of the 2D laser, and since it also has a viewing angle in the vertical direction, that is, a detection plane of more than a single layer, the boundary of the whole obstacle can be detected, and then appropriate obstacle avoidance control measures are made.
Meanwhile, the 3D laser radar adopted by the invention not only can be used for detecting the obstacle, but also can be used for SLAM (simultaneous localization and composition construction), human body detection and other various purposes, and the application range of the 3D laser radar is enlarged. And most of automatic navigation vehicles adopting the 3D laser radar for positioning can be directly used without adding other sensor equipment. In addition, the obstacle avoidance control measure is based on the edge information of the obstacle and the original planned path, so that the vehicle cannot swing back and forth due to the narrow place, and can always travel in the path closest to the original planned path, and cannot deviate from the path.
In one aspect, the invention provides a 3D laser-based obstacle detection and obstacle avoidance control method, which comprises the following steps: s1, clustering laser beams on the same plane, and classifying points measured by the laser beams into different classes; s2, fusing a plurality of clustered planes, and merging the range of the class of each plane to obtain the boundary range of each class; s3, further fusing the fused classes according to the boundary distance to obtain the position information and the range information of the obstacle with the boundary distance larger than the threshold distance; and S4, planning corresponding obstacle avoidance control measures based on the position information and the range information of the obstacle, the original planning path and the related information of the vehicle.
According to the above method, step S1 comprises: the points are assigned to different classes based on the Euclidean distance between the points measured by adjacent two laser beams.
According to the method described above, in step S4 the respective obstacle avoidance control measures are planned based on one or more of the following rules: (i) the vehicle is as close as possible to the original planned path; (ii) the vehicle cannot deviate from the original planned path; (iii) the distance the vehicle bypasses is as short as possible.
According to the above method, step S4 comprises: different thresholds are adopted for the detection distance and the obstacle avoidance control distance, so that the position information and the range information of the whole obstacle can be obtained before the obstacle avoidance control is triggered.
According to the above method, step S4 comprises: s4.1, screening out the boundary point of the barrier closest to the original planning path as an optimal boundary point; s4.2, reserving the width capable of passing through the vehicle according to the optimal boundary point, so as to obtain a reference point; s4.3, determining a target point of vehicle control based on the optimal boundary point and the reference point; and S4.4, triggering obstacle avoidance measures of the vehicle, and controlling the vehicle to move towards the target point.
According to the above method, step S4.1 comprises: and screening through angles formed between boundary coordinates of the left side and the right side of the obstacle, control points of the vehicle and fixed points on the original planning path, and selecting a boundary point with the minimum angle as the optimal boundary point.
According to the above method, step S4.2 comprises: the reference point is obtained by extending a preset distance from the optimal boundary point to the side far away from the obstacle along the direction perpendicular to the original planned path; wherein the predetermined distance is at least P/2, P representing the minimum width required to pass through the vehicle.
According to the above method, step S4.2 comprises: when the obstacle is closest to the vehicle in a direction along the originally planned path at a point hl min If not, the slave point hl min Extending a preset distance to one side of the optimal boundary point along a direction perpendicular to the original planning path to obtain the reference point; wherein the predetermined distance is at least P/2+V, P representing the minimum width required to pass through the vehicle; v represents a point hl min And the vertical distance between the original planned path and the point hv min Or point hv max And the absolute value of the difference in vertical distance between the original planned path, wherein the point hv min A point hv representing the closest point of the obstacle to the vehicle in a direction perpendicular to the original planned path max Representing the point of the obstacle furthest from the vehicle in a direction perpendicular to the original planned path.
According to the above method, step S4.3 comprises: if the optimal boundary point and the reference point are respectively positioned at two sides of the original planning path, taking the reference point as the target point; if the optimal boundary point and the reference point are located on the same side of the original planning path, and the angle formed between the optimal boundary point and the control point of the vehicle and the fixed point on the original planning path is larger than or equal to the angle formed between the reference point and the control point of the vehicle and the fixed point on the original planning path, taking the fixed point on the original planning path as the target point; and if the optimal boundary point and the reference point are positioned on the same side of the original planning path, and the angle formed between the optimal boundary point and the control point of the vehicle and the fixed point on the original planning path is smaller than the angle formed between the reference point and the control point of the vehicle and the fixed point on the original planning path, taking the reference point as the target point.
According to the above method, step S4.4 comprises: when the obstacle avoidance measures of the vehicle are triggered, the current target point is locked, and when the vehicle reaches the vicinity of the target point, the target point is unlocked, and the steps S4.1 to S4.3 are repeated to update the target point.
According to the above method, step S4.4 comprises: and updating the target point when the projection distance between the control point of the vehicle and the reference point in the direction along the original planned path is smaller than a threshold distance.
According to the above method, steps S4.1 to S4.4 are repeated until the vehicle bypasses the obstacle.
According to the above method, if an obstacle is found and the target point is changed, when a projected distance between the control point of the vehicle and the reference point in a direction along the original planned path is less than a threshold distance and the obstacle is detected in front, the control point of the vehicle is set to a point at which a projected point of the current control point on the original planned path extends forward by a predetermined distance.
In another aspect, the present invention provides a 3D laser-based obstacle detection and avoidance control system, including a 3D laser radar and a control device, where the 3D laser radar is installed on a vehicle and configured to be capable of acquiring information of an obstacle in front of the movement of the vehicle; and the control device is configured to be able to detect the obstacle and to perform obstacle avoidance control of the vehicle according to the method of any one of claims 1 to 13, based on the information of the obstacle acquired by the 3D lidar.
According to the above system, the 3D lidar is mounted on the vehicle according to a vertical angle of view of the 3D lidar and a height range of a body guard of the vehicle.
The obstacle avoidance control measure provided by the invention is based on the edge information of the obstacle and the original planned path, so that the vehicle cannot swing back and forth due to the narrow place, and can always travel in the path closest to the original planned path, and cannot deviate from the path. In addition, the 3D laser radar used for multiplexing and positioning can be used, and the obtained information of the obstacle is accurate and rich.
The conception, specific structure, and technical effects of the present invention will be further described with reference to the accompanying drawings to fully understand the objects, features, and effects of the present invention.
Drawings
FIG. 1A is a schematic view of a 3D laser according to an embodiment of the present invention;
FIG. 1B is a schematic view of a 3D laser according to an embodiment of the present invention;
FIG. 2 is a schematic clustering diagram of one embodiment of detecting obstacles in the present invention;
FIG. 3A is a schematic diagram of a path plan for one embodiment of obstacle avoidance control of the present invention, wherein the obstacle is closer to the original planned path;
FIG. 3B is a schematic diagram of a path plan for another embodiment of obstacle avoidance control of the present invention, wherein the obstacle is on the original planned path;
FIG. 3C is a schematic diagram of a path plan for another embodiment of obstacle avoidance control of the present invention, wherein the obstacle is further from the original planned path;
FIG. 4 is a flow chart of one embodiment of an obstacle detection process in the present invention;
FIG. 5 is a flow chart illustrating an embodiment of an obstacle avoidance control process according to the present invention.
Detailed Description
The following description of the preferred embodiments of the present invention refers to the accompanying drawings, which make the technical contents thereof more clear and easy to understand. The present invention may be embodied in many different forms of embodiments and the scope of the present invention is not limited to only the embodiments described herein.
In the following description, numerous specific details are set forth in order to provide a more thorough understanding of the present invention. It will be apparent, however, to one skilled in the art that the invention may be practiced without one or more of these details. It should be understood that the present invention may be embodied in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and convenient.
It is to be understood that, although directional terms, such as "front", "rear", "upper", "lower", "left", "right", etc., may be used in describing various exemplary structural portions and elements of the present invention, these terms are used herein for convenience of description only and are determined based on the exemplary orientations shown in the drawings. Since the disclosed embodiments of the invention may be arranged in a variety of orientations, these directional terms are used by way of illustration only and are in no way limiting.
The technical scheme of the present invention will be described in detail with reference to specific embodiments of the present invention.
The laser radar is a radar system for detecting the position, speed and other characteristic quantities of a target by emitting laser beams, and the 3D laser radar is a laser radar system for detecting by emitting laser beams with a certain angle of view, wherein the angle of view comprises a horizontal direction and a vertical direction, so that a 3D three-dimensional coverage surface is formed. As shown in fig. 1A, a schematic view of a vertical view angle is shown, which can detect a certain angle range in a vertical direction, wherein the vertical view angle is smaller than 180 degrees, usually smaller than 90 degrees, and the elevation view angle and the overlook view angle can be flexibly set according to needs, such as 45 degrees for each of the elevation view angle and the overlook view angle, or 60 degrees for the elevation view angle and 30 degrees for the overlook view angle. As shown in fig. 1B, a schematic view of a horizontal viewing angle is shown, a certain angle range can be detected in a horizontal direction, the horizontal viewing angle is smaller than 360 degrees, usually smaller than 270 degrees, and similarly, the viewing angles on the left side and the right side can be flexibly set as required.
The present invention is a control system for detecting an obstacle and avoiding the obstacle during automatic navigation of a vehicle. It is a very important and complex matter for positioning and navigation systems to detect and bypass obstacles in a certain way. It generally relates to the detection of obstacles, including the detection of the position, size, distance between a plurality of obstacles, and measures taken by the vehicle after the detection of an obstacle.
The obstacle detection and avoidance method for the vehicle provided by the invention has the advantages of high safety, high accuracy, high efficiency and high stability. The high safety means that the vehicle cannot collide with an obstacle; the high accuracy means that the position and the size of the obstacle are accurately detected; the high efficiency means that the optimal path can be found to control the vehicle to pass through after encountering the obstacle; high stability means that the obstacle avoidance measures taken do not cause the vehicle to deviate too far from the originally designed route.
In use, only the 3D lidar is required to be mounted on the vehicle, based on the vertical view of the 3D laser and the height range of the vehicle body guard. If the vehicle is higher, the vehicle can be arranged at the vehicle head and the vehicle tail at the same time, so that the 3D laser can scan lower obstacles; if the vehicle is lower, the vehicle can be only arranged at the front or the rear of the vehicle. Therefore, the 3D laser radar used for positioning can be conveniently multiplexed, and most automatic navigation vehicles adopting the 3D laser radar for positioning can be directly used without adding other sensor equipment.
The obstacle detection and obstacle avoidance control method mainly comprises two parts, namely obstacle recognition and obstacle avoidance control. The obstacle recognition is realized mainly by clustering according to Euclidean distances between position coordinates measured by two adjacent laser beams on the same horizontal plane and then fusing a plurality of laser planes. As shown in fig. 4, which is a flowchart of an embodiment of the obstacle detection process in the present invention, the method may specifically include the following steps:
(1) Clustering laser beams on the same plane: for the laser beams on the same plane, the coordinates measured by two adjacent laser beams are respectively [ x ] 0 y 0 z 0 ]And [ x ] 1 y 1 z 1 ]Then the Euclidean distance between the two points is
If the Euclidean distance is less than or equal to a certain threshold, i.e. T.ltoreq.T threshold Then the two points can be considered to belong to the same class, i.e. to the same object, or else to another class, i.e. to different objects. Thus, each obstacle can be divided by setting an appropriate threshold value. Fig. 2 is a schematic diagram of clustering, in which black points are laser points, and closed curves indicate that points inside the points belong to the same class, and clustering is performed according to the above manner to obtain four classes.
(2) Fusing a plurality of planes: fusion between multiple planes may be performed after each planar cluster is completed. The fusion between planes is carried out by adopting a mode of combining the clustering ranges of each plane, namely, each cluster has a certain boundary range, and class C is set 0 And C 1 Two classes of two different planes, the range of the corresponding x_y plane is h 0 And h 1 And h 0 ∩h 1 Not NULL, i.e. there is an intersection, then its total range is r=r 0 ∪r 1 。
Range h i ＝{hl min hl max hv min hv max }, where hl min Represents the coordinates closest to the current anchor point in the horizontal direction of the planned path, hl max Representing the coordinates, hv, furthest from the current anchor point in the horizontal direction of the planned path min Representing the nearest coordinate, hv, of the nearest point distance on the corresponding track from the boundary of the obstacle in the vertical direction of the planned path max Representing the coordinates in the vertical direction of the planned path where the boundary of the obstacle is furthest from the closest point on the corresponding trajectory.
(3) And further fusing the fused classes according to the boundary distance: if the distance between the boundaries of two classes is less than the minimum distance P that the vehicle can pass, i.e. the vehicle cannot pass between the two obstacles, then the two classes are further fused together, considered to be the same class, in the same way as the fusion between the two planes. In this way, the position information and the range information of the obstacle having the boundary distance greater than P can be finally obtained.
After the identification of the obstacle is completed, corresponding obstacle avoidance measures are taken, and an obstacle avoidance control strategy is planned mainly by the aid of the original planned path and the position and the range of the identified obstacle.
Fig. 3A-3C are schematic diagrams of three different types of path planning in the present invention, respectively showing corresponding path planning for different situations of an obstacle. Wherein, obtaining the obstacle through clustering, and representing boundary coordinates of the obstacle as r0, r1, r2 … … rn; the vehicle 100, on which the O-point represents a control point; arrow D represents the original planned path; setpoint a is a point on the forward path of vehicle 100. In fig. 3A, the obstacles 101 and 102 are obtained through clustering, and are closer to the original planned path, wherein the boundary coordinates of the obstacle 101 are denoted as r0 and r1, and the boundary coordinates of the obstacle 102 are denoted as r2 and r 3; in fig. 3B, the obstacle 103 is obtained by clustering, and on the original planned path, its boundary coordinates are denoted as r0, r1, and r2; in fig. 3C, the obstacle 104 is obtained by clustering, which is far from the original planned path, and its boundary coordinates are denoted as r0 and r1.
The planning of obstacle avoidance control measures is mainly based on the following points:
(i) The vehicle is as close to the original planned path as possible;
(ii) The vehicle cannot deviate from the original planned path;
(iii) The distance the vehicle bypasses is as short as possible.
In order to reduce the number of path planning and increase the stability of vehicle control, different thresholds can be adopted for the detection distance (the range of 3D laser detection) and the obstacle avoidance control distance (the trigger obstacle avoidance control strategy), tl represents the distance of forward laser detection, and Tc represents the distance of vehicle trigger control. By selecting proper Tl and Tc, the position and range information of the whole obstacle can be obtained before the vehicle triggers obstacle avoidance control, so that the problem of insufficient vehicle control quantity caused by the fact that the obstacle avoidance control is triggered only by scanning part of the obstacle is avoided.
Fig. 5 is a flowchart of an embodiment of an obstacle avoidance control process according to the present invention, which may specifically include the following steps:
(1) Screening out the boundary of the obstacle closest to the original planned path: the screening can be performed by the angle formed between the boundary coordinates of the left and right sides of the obstacle, the vehicle control point O and the fixed point A on the original planning path, the smaller the formed angle is, the closer the formed angle is to the original planning path, wherein the point closest to the original planning path is taken as the optimal boundary point, namely min (< r) is selected 0 OA,∠r 1 OA,∠r 2 OA,∠r 3 OA...∠r n OA) as the optimal boundary point. In FIG. 3A, +.r 1 OA is the smallest, so the boundary r1 of the obstacle 101 is closest to the original planned path, and the point r1 can be selected as the optimal boundary point for subsequent planning. Similarly, in fig. 3B, r0 is selected as the optimal boundary point; in fig. 3C, r1 is selected as the optimal boundary point. Because the optimal boundary point is the boundary point closest to the original planning path, the motion path which is planned based on the optimal boundary point and bypasses the obstacle can be as close to the original planning path as possible.
(2) After the optimal boundary point is selected, the width P through which the vehicle can pass is reserved to allow the vehicle to pass. If the optimal boundary point ri of the selected obstacle is the left boundary of the belonging obstacle, extending at least P/2 leftwards through the point ri in the direction perpendicular to the original planned path D, as shown in fig. 3B, extending P/2 leftwards from r 0; if ri is the right boundary of the obstacle, then at least P/2 is extended to the right, extending P/2 from r1 to the right as shown in FIGS. 3A and 3C. More specifically, in fig. 3A and 3C, r1 belongs to the right boundary of the obstacle 101, so P/2 is extended rightward by r1 point in the direction perpendicular to the original planned path D, reaching the reference point AP. In fig. 3B, the obstacle 103 is a trapezoidal obstacle, and after the optimal boundary point is selected, the closest point hl of the vehicle is further located in the direction of the original planned path D min (i.e., point r1 in FIG. 3B), extends at least P/2+V to reference point AP in a direction perpendicular to original planned path D, where V represents closest point hl min Perpendicular distance from original planned path, and hv min Or hv max And the absolute value of the difference between the vertical distances of the original planned path. I.e., from point r1 to point AP in FIG. 3BIn V is equal to the absolute value of the difference between the distance of closest points r1 and F0 and the distance of r0 and F1. According to the minimum width P through which the vehicle can pass and the relation between the obstacle and the original planned path, the planned movement distance around the obstacle can be as short as possible.
(3) The target point of the vehicle control is determined based on the optimal boundary point and the reference point. If the optimal boundary point ri of the obstacle and the reference point AP determined after extension are both on the same side of the original planned path, and +.r i OA- < APOA > 0, which indicates that the obstacle does not affect the vehicle to advance along the original planned path, then the target point of the vehicle control can be set as the fixed point A on the original planned path, as shown in FIG. 3C, that is, the vehicle is running along the original planned path; if ri and AP are on both sides of the original planned path, as shown in FIG. 3A, illustrating that the obstacle can affect the vehicle to advance along the original planned path, setting the reference point AP as a target point for vehicle control; if ri and AP are both on the same side of the original planned path, however +.r i If OA & lt, APOA & lt, 0, as shown in FIG. 3B, it is indicated that the obstacle will affect the vehicle to advance along the original planned path, and the reference point AP is set as the target point of vehicle control. Determining the point A or the AP as a target point of vehicle control can trigger obstacle avoidance control of the vehicle.
(4) Triggering obstacle avoidance control of the vehicle, controlling the vehicle to move towards the target point, and updating the target point in the moving process. If obstacle avoidance control of the vehicle is triggered, the current target point is locked and not updated until the current vector is
The projection of the original planning path is smaller than a certain threshold value P threshold When the vehicle reaches the vicinity of the control target point, unlocking is started, the target point of the obstacle avoidance control of the vehicle is updated, and algorithm circulation control is sequentially performed. As shown in fig. 3A and 3B, the curve between AP and APn is a path line formed by the control target points during the vehicle passing around the obstacle.
In order to reduce the delay of control and to make the control smoother, if an obstacle is found and the target of the original vehicle control is changedWhen a point is present, then
The projection of the original planning path is smaller than a certain threshold value P threshold And an obstacle is detected in front, the control point may be set to a point where the projected point of the current control point on the original planned path extends forward by a distance H, as in point A1 in fig. 3A and 3B.
Thus, the control point of the obstacle avoidance control adopted in fig. 3A and 3B is O, AP, APn, A, and the curve part is the approximate track of the vehicle control, so that the complete obstacle avoidance control is completed. By continuously updating the target point and the control point during the movement, the vehicle can be prevented from deviating from the original planned path and the detour distance is as short as possible.
The invention also provides a barrier detection and obstacle avoidance control system based on the 3D laser, which comprises a 3D laser radar and a control device. Wherein, 3D laser radar installs on the vehicle, can obtain the information of the obstacle in front of the vehicle movement through the scanning of 3D laser. The control device can calculate and control the obstacle according to the obstacle detection and obstacle avoidance control method based on the information of the obstacle acquired by the 3D laser radar, so that the vehicle bypasses the obstacle. In various embodiments, the control device can be integrated with the 3D laser radar, can be independently installed and configured, and can be connected and communicated with the 3D laser radar in a wired or wireless mode; the control device can be installed on a vehicle to be controlled, can be independently installed and configured, and is connected and communicated with the vehicle to be controlled in a wired or wireless mode; the control device may be paired with or bound to a vehicle to be controlled, and may control only one specific vehicle, or may control a plurality of vehicles at the same time.
The foregoing describes in detail preferred embodiments of the present invention. It should be understood that numerous modifications and variations can be made in accordance with the concepts of the invention without requiring creative effort by one of ordinary skill in the art. Therefore, all technical solutions which can be obtained by logic analysis, reasoning or limited experiments based on the prior art by the person skilled in the art according to the inventive concept shall be within the scope of protection defined by the claims.
Claims (14)
1. The obstacle detection and obstacle avoidance control method based on the 3D laser is characterized by comprising the following steps of:
s1, clustering laser beams on the same plane, and classifying points measured by the laser beams into different classes;
s2, fusing a plurality of clustered planes, and merging the range of the class of each plane to obtain the boundary range of each class;
s3, further fusing the fused classes according to the boundary distance to obtain the position information and the range information of the obstacle with the boundary distance larger than the threshold distance;
s4, planning an obstacle avoidance control strategy based on the original planning path and the identified position and range of the obstacle, wherein the obstacle avoidance control strategy comprises the following steps:
s4.1, screening out the boundary point of the barrier closest to the original planning path as an optimal boundary point;
s4.2, reserving the width of the vehicle passing through according to the optimal boundary point to obtain a reference point;
s4.3, determining a target point of vehicle control based on the optimal boundary point and the reference point;
and S4.4, triggering obstacle avoidance measures of the vehicle, and controlling the vehicle to move towards the target point.
2. The method according to claim 1, wherein step S1 comprises:
points measured by two adjacent laser beams are assigned to different classes based on their Euclidean distance between the points.
3. The method according to claim 1, characterized in that in step S4 the respective obstacle avoidance control measures are planned based on one or more of the following rules:
(i) The vehicle is as close to the original planned path as possible;
(ii) The vehicle cannot deviate from the original planned path;
(iii) The distance the vehicle bypasses is as short as possible.
4. The method according to claim 1, wherein step S4 comprises:
different thresholds are adopted for the detection distance and the obstacle avoidance control distance, so that the position information and the range information of the whole obstacle can be obtained before the obstacle avoidance control is triggered.
5. The method according to claim 1, wherein step S4.1 comprises:
and screening through angles formed between boundary coordinates of the left side and the right side of the obstacle, control points of the vehicle and fixed points on the original planning path, and selecting a boundary point with the minimum angle as the optimal boundary point.
6. The method according to claim 1, wherein step S4.2 comprises:
the reference point is obtained by extending a preset distance from the optimal boundary point to the side far away from the obstacle along the direction perpendicular to the original planned path;
wherein the predetermined distance is at least P/2, P representing the minimum width required to pass through the vehicle.
7. The method according to claim 1, wherein step S4.2 comprises:
when the obstacle is closest to the vehicle in a direction along the original planned path, a point hl min If not, the slave point hl min Extending a preset distance to one side of the optimal boundary point along a direction perpendicular to the original planning path to obtain the reference point;
wherein the predetermined distance is at least P/2+V, P representing the minimum width required to pass through the vehicle; v represents a point hl min And the vertical distance between the original planned path and the point hv min Or point hv max And the absolute value of the difference in vertical distance between the original planned path, wherein the point hv min A point hv representing the closest point of the obstacle to the vehicle in a direction perpendicular to the original planned path max Representing the point of the obstacle furthest from the vehicle in a direction perpendicular to the original planned path.
8. The method according to claim 5, wherein step S4.3 comprises:
if the optimal boundary point and the reference point are respectively positioned at two sides of the original planning path, taking the reference point as the target point;
if the optimal boundary point and the reference point are located on the same side of the original planning path, and the angle formed between the optimal boundary point and the control point of the vehicle and the fixed point on the original planning path is larger than or equal to the angle formed between the reference point and the control point of the vehicle and the fixed point on the original planning path, taking the fixed point on the original planning path as the target point; and
and if the optimal boundary point and the reference point are positioned on the same side of the original planning path, and the angle formed between the optimal boundary point and the control point of the vehicle and the fixed point on the original planning path is smaller than the angle formed between the reference point and the control point of the vehicle and the fixed point on the original planning path, taking the reference point as the target point.
9. The method according to claim 1, wherein step S4.4 comprises:
when the obstacle avoidance measures of the vehicle are triggered, the current target point is locked, and when the vehicle reaches the vicinity of the target point, the target point is unlocked, and the steps S4.1 to S4.3 are repeated to update the target point.
10. The method according to claim 9, wherein step S4.4 comprises:
and updating the target point when the projection distance between the control point of the vehicle and the reference point in the direction along the original planning path is smaller than a threshold distance.
11. The method according to claim 9, wherein:
steps S4.1 to S4.4 are repeated until the vehicle bypasses the obstacle.
12. The method according to claim 1, characterized in that:
if an obstacle is found and the target point is changed, when a projected distance between the control point of the vehicle and the reference point in a direction along the original planned path is less than a threshold distance and the obstacle is detected in front, the control point of the vehicle is set to a point where a projected point of the current control point on the original planned path extends forward by a predetermined distance.
13. Obstacle detection and obstacle avoidance control system based on 3D laser, including 3D laser radar and controlling means, its characterized in that:
the 3D lidar is mounted on a vehicle and configured to be able to acquire information of an obstacle in front of the movement of the vehicle; and
the control device is configured to be able to detect the obstacle and to perform obstacle avoidance control of the vehicle according to the method of any one of claims 1 to 12, based on the information of the obstacle acquired by the 3D lidar.
14. The system according to claim 13, wherein:
the 3D lidar is mounted on the vehicle according to a vertical viewing angle of the 3D lidar and a height range of a body guard of the vehicle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810306723.2A CN110346814B (en) | 2018-04-08 | 2018-04-08 | Obstacle detection and obstacle avoidance control method and system based on 3D laser |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810306723.2A CN110346814B (en) | 2018-04-08 | 2018-04-08 | Obstacle detection and obstacle avoidance control method and system based on 3D laser |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110346814A CN110346814A (en) | 2019-10-18 |
| CN110346814B true CN110346814B (en) | 2023-04-28 |
Family
ID=68173156
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810306723.2A Active CN110346814B (en) | 2018-04-08 | 2018-04-08 | Obstacle detection and obstacle avoidance control method and system based on 3D laser |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110346814B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111256719B (en) * | 2020-02-18 | 2022-02-22 | 北京九曜智能科技有限公司 | Obstacle detouring method and device |
| CN111897361B (en) * | 2020-08-05 | 2023-08-22 | 广州市赛皓达智能科技有限公司 | Unmanned aerial vehicle autonomous route planning method and system |
| CN114167851B (en) * | 2020-09-10 | 2024-04-30 | 北京极智嘉科技股份有限公司 | Robot and robot travelling method |
| CN116457698A (en) * | 2020-11-30 | 2023-07-18 | 华为技术有限公司 | Laser radar and mobile platform |
| CN112379679B (en) * | 2021-01-15 | 2021-04-23 | 北京理工大学 | Unmanned vehicle local path planning method |
| CN113377102A (en) * | 2021-04-29 | 2021-09-10 | 中联重科土方机械有限公司 | Control method, processor and device for excavator and excavator |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010061442A (en) * | 2008-09-04 | 2010-03-18 | Murata Machinery Ltd | Autonomous mobile device |
| JP2014010797A (en) * | 2012-07-03 | 2014-01-20 | Murata Mach Ltd | Autonomous traveling device |
| CN105974917A (en) * | 2016-05-11 | 2016-09-28 | 江苏大学 | Vehicle obstacle-avoidance path planning research method based on novel manual potential field method |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8073564B2 (en) * | 2006-07-05 | 2011-12-06 | Battelle Energy Alliance, Llc | Multi-robot control interface |
| US7668621B2 (en) * | 2006-07-05 | 2010-02-23 | The United States Of America As Represented By The United States Department Of Energy | Robotic guarded motion system and method |
| US7584020B2 (en) * | 2006-07-05 | 2009-09-01 | Battelle Energy Alliance, Llc | Occupancy change detection system and method |
| AT507035B1 (en) * | 2008-07-15 | 2020-07-15 | Airbus Defence & Space Gmbh | SYSTEM AND METHOD FOR AVOIDING COLLISION |
| JP5385009B2 (en) * | 2009-05-29 | 2014-01-08 | 日立オートモティブシステムズ株式会社 | Vehicle control apparatus and vehicle control method |
-
2018
- 2018-04-08 CN CN201810306723.2A patent/CN110346814B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010061442A (en) * | 2008-09-04 | 2010-03-18 | Murata Machinery Ltd | Autonomous mobile device |
| JP2014010797A (en) * | 2012-07-03 | 2014-01-20 | Murata Mach Ltd | Autonomous traveling device |
| CN105974917A (en) * | 2016-05-11 | 2016-09-28 | 江苏大学 | Vehicle obstacle-avoidance path planning research method based on novel manual potential field method |
Non-Patent Citations (2)
| Title |
|---|
| 《Obstacle Detection and Road Following using Laser Scanner》;Zezhong Xu et.al;《2006 6th World Congress on Intelligent Control and Automation》;20060623;第2卷;第8630-8634页 * |
| 《基于视觉的道路跟踪》;杨明 等;《模式识别与人工智能》;20010630;第14卷(第2期);第186-193页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110346814A (en) | 2019-10-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110346814B (en) | Obstacle detection and obstacle avoidance control method and system based on 3D laser | |
| CN108693878B (en) | Course setting device and course setting method | |
| US11313976B2 (en) | Host vehicle position estimation device | |
| JP6800168B2 (en) | How to estimate the driving lane | |
| US10493984B2 (en) | Vehicle control method and vehicle control device | |
| US9229450B2 (en) | Autonomous movement system | |
| CN110103967B (en) | Automatic lane changing method for vehicle, vehicle control system and vehicle | |
| JP3860061B2 (en) | Outside-of-vehicle monitoring device and travel control device equipped with this out-of-vehicle monitoring device | |
| KR101926322B1 (en) | Vehicle position estimating apparatus, vehicle position estimating method | |
| US10365657B2 (en) | Autonomous moving object | |
| CN108536149A (en) | A kind of automatic driving vehicle avoidance obstacle device and control method based on the paths Dubins | |
| CN112327878B (en) | Obstacle classification and obstacle avoidance control method based on TOF camera | |
| CN110816548A (en) | Sensor fusion | |
| WO2013027803A1 (en) | Autonomous driving control system for vehicle | |
| JP6881464B2 (en) | Self-position estimation method and self-position estimation device | |
| CN115220033A (en) | Automated vehicle object detection system with camera image and radar data fusion | |
| JP7077967B2 (en) | Driving lane estimation device, driving lane estimation method, and control program | |
| JP2004531424A (en) | Sensing device for cars | |
| CN111273673A (en) | Automatic driving following method and system of unmanned vehicle and unmanned vehicle | |
| US11299176B2 (en) | Vehicle control device | |
| US11285953B2 (en) | Determining visibility distances based on a dynamic field of view of a vehicle | |
| CN112363513A (en) | Obstacle classification and obstacle avoidance control method based on depth information | |
| CN112937606A (en) | Anti-collision path planning and control method and system for tracking automatic driving vehicle | |
| CN114728657A (en) | Vehicle control method and vehicle control device | |
| CN112581771B (en) | Driving control device, parking target, and driving control system for automatic driving vehicle |
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 |