CN113296118B - Unmanned obstacle detouring method and terminal based on laser radar and GPS - Google Patents

Unmanned obstacle detouring method and terminal based on laser radar and GPS Download PDF

Info

Publication number
CN113296118B
CN113296118B CN202110562594.5A CN202110562594A CN113296118B CN 113296118 B CN113296118 B CN 113296118B CN 202110562594 A CN202110562594 A CN 202110562594A CN 113296118 B CN113296118 B CN 113296118B
Authority
CN
China
Prior art keywords
obstacle
coordinates
gps
point
laser radar
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110562594.5A
Other languages
Chinese (zh)
Other versions
CN113296118A (en
Inventor
余平
林立言
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu Shenghai Intelligent Technology Co ltd
Original Assignee
Jiangsu Shenghai Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu Shenghai Intelligent Technology Co ltd filed Critical Jiangsu Shenghai Intelligent Technology Co ltd
Priority to CN202110562594.5A priority Critical patent/CN113296118B/en
Publication of CN113296118A publication Critical patent/CN113296118A/en
Application granted granted Critical
Publication of CN113296118B publication Critical patent/CN113296118B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders

Abstract

The invention discloses an unmanned obstacle detouring method and a terminal based on a laser radar and a GPS; the method starts the laser radar and controls the vehicle to run along the pre-acquired path track according to the current GPS information; performing obstacle recognition according to point cloud data of a laser radar, acquiring three-dimensional data of an obstacle and obstacle laser radar coordinates, calculating an obstacle GPS (global positioning system) coordinate, judging whether obstacle detouring is required according to the obstacle GPS coordinate and the three-dimensional data, generating an obstacle detouring route according to a preset obstacle detouring algorithm when a vehicle is away from the obstacle by a preset distance if obstacle detouring is required, and driving along the obstacle detouring route until the obstacle detours and returns to the pre-acquired path track; the invention can realize the automatic obstacle avoidance of unmanned tracking driving of the vehicle, judge based on GPS coordinates and three-dimensional data, and judge whether the position and the height of the comprehensive obstacle can cause harm to the vehicle, thereby reducing unnecessary avoidance and improving the obstacle avoidance efficiency.

Description

Unmanned obstacle detouring method and terminal based on laser radar and GPS
Technical Field
The invention relates to the technical field of unmanned aerial vehicle, in particular to an unmanned obstacle detouring method and terminal based on a laser radar and a GPS.
Background
Unmanned driving is a very common way in unmanned vehicle autopilot. Unmanned driving refers to automatic tracking driving of an automobile according to a preset route. When an obstacle appears on the unmanned path and the vehicle cannot actively avoid the obstacle, accidents such as collision, rollover and the like occur, so that economic loss can be caused, task execution failure can be caused, and the use range and the service life of the unmanned vehicle are limited.
Disclosure of Invention
The technical problems to be solved by the invention are as follows: the unmanned obstacle detouring method and the terminal based on the laser radar and the GPS can realize automatic obstacle detouring of unmanned tracking driving and improve obstacle detouring efficiency.
In order to solve the technical problems, the invention adopts the following technical scheme:
an unmanned obstacle detouring method based on a laser radar and a GPS, comprising:
s1, starting a laser radar, and controlling a vehicle to run along a pre-acquired path track according to current GPS information;
s2, performing obstacle recognition according to point cloud data of a laser radar, acquiring three-dimensional data of an obstacle and obstacle laser radar coordinates, calculating an obstacle GPS (global positioning system) coordinate according to the obstacle laser radar coordinates, judging whether obstacle detouring is needed according to the obstacle GPS coordinate and the three-dimensional data, entering a step S3 if obstacle detouring is needed, and otherwise continuing to run along a path track;
and S3, when the vehicle is at a preset distance from the obstacle, generating an obstacle detouring route according to a preset obstacle detouring algorithm, and driving along the obstacle detouring route until the vehicle detours the obstacle and returns to the pre-acquired path track.
In order to solve the technical problems, the invention adopts another technical scheme that:
an unmanned obstacle detouring terminal based on a laser radar and a GPS, comprising a processor, a memory and a computer program stored on the memory and operable on the processor, wherein the processor implements the following steps when executing the computer program:
s1, starting a laser radar, and controlling a vehicle to run along a pre-acquired path track according to current GPS information;
s2, performing obstacle recognition according to point cloud data of a laser radar, acquiring three-dimensional data of an obstacle and obstacle laser radar coordinates, calculating an obstacle GPS (global positioning system) coordinate according to the obstacle laser radar coordinates, judging whether obstacle detouring is needed according to the obstacle GPS coordinate and the three-dimensional data, entering a step S3 if obstacle detouring is needed, and otherwise continuing to run along a path track;
and S3, when the vehicle is at a preset distance from the obstacle, generating an obstacle detouring route according to a preset obstacle detouring algorithm, and driving along the obstacle detouring route until the vehicle detours the obstacle and returns to the pre-acquired path track.
The invention has the beneficial effects that: according to the invention, the GPS coordinates and the three-dimensional data of the obstacle are determined according to the laser radar point cloud data, and whether the obstacle damages the vehicle or not is judged according to the laser radar coordinates and the three-dimensional data, so that the automatic obstacle avoidance of unmanned driving of the vehicle is realized.
Drawings
FIG. 1 is a flow chart of an unmanned obstacle detouring method based on a laser radar and a GPS according to an embodiment of the invention;
FIG. 2 is a block diagram of an unmanned obstacle detouring terminal based on lidar and GPS according to an embodiment of the present invention;
FIG. 3 is a detailed flow chart of an unmanned obstacle detouring method based on laser radar and GPS according to an embodiment of the invention;
FIG. 4 is a schematic diagram of an unmanned obstacle detouring method based on lidar and GPS according to an embodiment of the present invention;
description of the reference numerals:
1. unmanned obstacle detouring terminal based on laser radar and GPS; 2. a processor; 3. a memory.
Detailed Description
In order to describe the technical contents, the achieved objects and effects of the present invention in detail, the following description will be made with reference to the embodiments in conjunction with the accompanying drawings.
Referring to fig. 1, 3 and 4, an unmanned obstacle detouring method based on a laser radar and a GPS includes:
s1, starting a laser radar, and controlling a vehicle to run along a pre-acquired path track according to current GPS information;
s2, performing obstacle recognition according to point cloud data of a laser radar, acquiring three-dimensional data of an obstacle and obstacle laser radar coordinates, calculating an obstacle GPS (global positioning system) coordinate according to the obstacle laser radar coordinates, judging whether obstacle detouring is needed according to the obstacle GPS coordinate and the three-dimensional data, entering a step S3 if obstacle detouring is needed, and otherwise continuing to run along a path track;
and S3, when the vehicle is at a preset distance from the obstacle, generating an obstacle detouring route according to a preset obstacle detouring algorithm, and driving along the obstacle detouring route until the vehicle detours the obstacle and returns to the pre-acquired path track.
From the above description, the beneficial effects of the invention are as follows: according to the invention, the GPS coordinates and the three-dimensional data of the obstacle are determined according to the laser radar point cloud data, and whether the obstacle damages the vehicle or not is judged according to the laser radar coordinates and the three-dimensional data, so that the automatic obstacle avoidance of unmanned driving of the vehicle is realized.
Further, in the step S2, the obstacle recognition is performed according to the point cloud data of the lidar, and the three-dimensional data of the obstacle and the coordinates of the lidar of the obstacle are specifically:
traversing point cloud data of a laser radar, calculating point distances between points, storing two points with the point distances smaller than a preset threshold value into the same point set, wherein each point set is a set of all points contained in each obstacle;
calculating the length L of the barrier according to the maximum abscissa and the minimum abscissa, calculating the width W of the barrier according to the maximum ordinate and the minimum abscissa, and calculating the height H of the barrier according to the maximum ordinate and the minimum ordinate in the coordinates of all points of the point set to obtain three-dimensional data of the barrier;
and calculating the coordinates of the obstacle laser radar according to a clustering algorithm, wherein the coordinates of the obstacle laser radar comprise the coordinates of a radar center point and the coordinates of radar edge points.
From the above description, according to the point cloud data of the lidar, the obstacle can be accurately identified by the distance between the points, and the three-dimensional data of the obstacle and the coordinates of the obstacle lidar can be obtained by calculation for the subsequent steps.
Further, in the step S2, calculating the obstacle GPS coordinates according to the obstacle lidar coordinates specifically includes:
the current GPS information of the vehicle is acquired, wherein the current GPS information comprises real-time GPS coordinates (lon, lat) and a course angle theta, and the obstacle laser radar coordinates (x, y) are substituted into the following formula:
x′＝xcosθ-ysinθ
y′＝xsinθ+ycosθ
a first coordinate (x ', y ') can be obtained, where x, y, x ' and y ' are all in m, and based on 1m= 0.00054054054' and the real-time GPS coordinate (lon, lat), it is possible to obtain:
lon'＝lon+x'×0.00054054054'
lat'＝lat+y'×0.00054054054'
thereby obtaining obstacle GPS coordinates (lon ', lat').
From the above description, it is known that the obstacle GPS coordinates can be calculated according to the current GPS coordinates, heading angle and obstacle lidar coordinates of the vehicle, so as to determine whether the obstacle is within the dangerous range of the route track.
Further, the step of judging whether the obstacle is needed to be wound according to the GPS coordinates and the three-dimensional data comprises the following steps:
judging whether a point (x_t, y_t) is located at a distance from the path track which is smaller than a preset dangerous distance and H is larger than a preset dangerous height according to the GPS coordinates of the obstacle, if so, carrying out obstacle detouring, otherwise, not carrying out obstacle detouring;
wherein x is min ≤x_t≤x max ，y min ≤y_t≤y max Wherein x is min 、x max 、y min And y max And the minimum abscissa, the maximum abscissa, the minimum ordinate and the maximum ordinate of the GPS edge point coordinates of the obstacle GPS coordinates are respectively.
From the above description, it can be seen that whether the obstacle is located within the dangerous distance of the path track and whether the height of the obstacle is higher than the preset dangerous height is determined whether to avoid, that is, the position information and the height information are comprehensively considered, so that unnecessary avoidance can be effectively reduced, and the avoidance efficiency is improved.
Further, the path track includes a plurality of ordered target track points, and the step S3 specifically includes:
taking a value obtained by adding a preset safety interval to the maximum abscissa of the edge points in the obstacle laser radar coordinates as a maximum value x of obstacle-detouring abscissas m ；
When the vehicle is at a preset distance from the obstacle, according to the radar center point coordinates and the radar edge point coordinates, taking a value interval (0, x) in the abscissa at a preset interval m ]The abscissa x1 is substituted into the following formula:
(x1-r) 2 +y1 2 ＝r*r
determining a plurality of turning radar track points (x 1, y 1), wherein r is a preset turning radius of the vehicle, and the preset distance is greater than the turning radius r of the vehicle plus a preset safety distance;
by the turning radar track point B (x m ，y b ) Determining a point C (x m ，y c ) Wherein y is c Adding a preset safety distance to the maximum ordinate of the radar edge point coordinates, and determining a point D at a position behind the obstacle, which is larger than a preset dangerous distance, so as to control the vehicle to travel to the turning radar track point B along the turning radar track point B and then along the turning radar track point B->C->And D, continuing unmanned tracking driving according to the current GPS information and the path track after the point D is reached.
As can be seen from the above description, when the obstacle detouring is performed, the safety distance between each section of the driving path and the obstacle is considered, so that the obstacle detouring path is planned, the distance between the obstacle detouring path and the obstacle is always greater than or equal to the preset safety distance in the obstacle detouring process, and the safety of the vehicle is ensured.
Referring to fig. 2, an unmanned obstacle detouring terminal based on a laser radar and a GPS includes a processor, a memory and a computer program stored in the memory and executable on the processor, wherein the processor implements the following steps when executing the computer program:
s1, starting a laser radar, and controlling a vehicle to run along a pre-acquired path track according to current GPS information;
s2, performing obstacle recognition according to point cloud data of a laser radar, acquiring three-dimensional data of an obstacle and obstacle laser radar coordinates, calculating an obstacle GPS (global positioning system) coordinate according to the obstacle laser radar coordinates, judging whether obstacle detouring is needed according to the obstacle GPS coordinate and the three-dimensional data, entering a step S3 if obstacle detouring is needed, and otherwise continuing to run along a path track;
and S3, when the vehicle is at a preset distance from the obstacle, generating an obstacle detouring route according to a preset obstacle detouring algorithm, and driving along the obstacle detouring route until the vehicle detours the obstacle and returns to the pre-acquired path track.
From the above description, the beneficial effects of the invention are as follows: according to the invention, the GPS coordinates and the three-dimensional data of the obstacle are determined according to the laser radar point cloud data, and whether the obstacle damages the vehicle or not is judged according to the laser radar coordinates and the three-dimensional data, so that the automatic obstacle avoidance of unmanned driving of the vehicle is realized.
Further, in the step S2, the obstacle recognition is performed according to the point cloud data of the lidar, and the three-dimensional data of the obstacle and the coordinates of the lidar of the obstacle are specifically:
traversing point cloud data of a laser radar, calculating point distances between points, storing two points with the point distances smaller than a preset threshold value into the same point set, wherein each point set is a set of all points contained in each obstacle;
calculating the length L of the barrier according to the maximum abscissa and the minimum abscissa, calculating the width W of the barrier according to the maximum ordinate and the minimum abscissa, and calculating the height H of the barrier according to the maximum ordinate and the minimum ordinate in the coordinates of all points of the point set to obtain three-dimensional data of the barrier;
and calculating the coordinates of the obstacle laser radar according to a clustering algorithm, wherein the coordinates of the obstacle laser radar comprise the coordinates of a radar center point and the coordinates of radar edge points.
From the above description, according to the point cloud data of the lidar, the obstacle can be accurately identified by the distance between the points, and the three-dimensional data of the obstacle and the coordinates of the obstacle lidar can be obtained by calculation for the subsequent steps.
Further, in the step S2, calculating the obstacle GPS coordinates according to the obstacle lidar coordinates specifically includes:
the current GPS information of the vehicle is acquired, wherein the current GPS information comprises real-time GPS coordinates (lon, lat) and a course angle theta, and the obstacle laser radar coordinates (x, y) are substituted into the following formula:
x′＝xcosθ-ysinθ
y′＝xsinθ+ycosθ
a first coordinate (x ', y ') can be obtained, where x, y, x ' and y ' are all in m, and based on 1m= 0.00054054054' and the real-time GPS coordinate (lon, lat), it is possible to obtain:
lon'＝lon+x'×0.00054054054'
lat'＝lat+y'×0.00054054054'
thereby obtaining obstacle GPS coordinates (lon ', lat').
From the above description, it is known that the obstacle GPS coordinates can be calculated according to the current GPS coordinates, heading angle and obstacle lidar coordinates of the vehicle, so as to determine whether the obstacle is within the dangerous range of the route track.
Further, the step of judging whether the obstacle is needed to be wound according to the GPS coordinates and the three-dimensional data comprises the following steps:
judging whether a point (x_t, y_t) is located at a distance from the path track which is smaller than a preset dangerous distance and H is larger than a preset dangerous height according to the GPS coordinates of the obstacle, if so, carrying out obstacle detouring, otherwise, not carrying out obstacle detouring;
wherein x is min ≤x_t≤x max ，y min ≤y_t≤y max Wherein x is min 、x max 、y min And y max And the minimum abscissa, the maximum abscissa, the minimum ordinate and the maximum ordinate of the GPS edge point coordinates of the obstacle GPS coordinates are respectively.
From the above description, it can be seen that whether the obstacle is located within the dangerous distance of the path track and whether the height of the obstacle is higher than the preset dangerous height is determined whether to avoid, that is, the position information and the height information are comprehensively considered, so that unnecessary avoidance can be effectively reduced, and the avoidance efficiency is improved.
Further, the path track includes a plurality of ordered target track points, and the step S3 specifically includes:
taking a value obtained by adding a preset safety interval to the maximum abscissa of the edge points in the obstacle laser radar coordinates as a maximum value x of obstacle-detouring abscissas m ；
When the vehicle is at a preset distance from the obstacle, according to the radar center point coordinates and the radar edge point coordinates, taking a value interval (0, x) in the abscissa at a preset interval m ]The abscissa x1 is substituted into the following formula:
(x1-r) 2 +y1 2 ＝r*r
determining a plurality of turning radar track points (x 1, y 1), wherein r is a preset turning radius of the vehicle, and the preset distance is greater than the turning radius r of the vehicle plus a preset safety distance;
by the turning radar track point B (x m ，y b ) Determining a point C (x m ，y c ) Wherein y is c Adding a preset safety distance to the maximum ordinate of the radar edge point coordinates, wherein the preset safety distance is larger than a preset dangerous distance behind an obstacleTo control the vehicle to travel along the turning radar locus point to the turning radar locus point B and then along the line B->C->And D, continuing unmanned tracking driving according to the current GPS information and the path track after the point D is reached.
As can be seen from the above description, when the obstacle detouring is performed, the safety distance between each section of the driving path and the obstacle is considered, so that the obstacle detouring path is planned, the distance between the obstacle detouring path and the obstacle is always greater than or equal to the preset safety distance in the obstacle detouring process, and the safety of the vehicle is ensured.
Referring to fig. 1, 3 and 4, a first embodiment of the present invention is as follows:
an unmanned obstacle detouring method based on a laser radar and a GPS, comprising:
s1, starting a laser radar, and controlling a vehicle to run along a pre-acquired path track according to current GPS information;
s2, performing obstacle recognition according to point cloud data of a laser radar, acquiring three-dimensional data of an obstacle and obstacle laser radar coordinates, calculating an obstacle GPS (global positioning system) coordinate according to the obstacle laser radar coordinates, judging whether obstacle detouring is needed according to the obstacle GPS coordinate and the three-dimensional data, entering a step S3 if obstacle detouring is needed, and otherwise continuing to run along a path track;
in the step S2, the obstacle recognition is performed according to the point cloud data of the lidar, and the three-dimensional data of the obstacle and the coordinates of the lidar of the obstacle are specifically:
traversing point cloud data of a laser radar, calculating point distances between points, storing two points with the point distances smaller than a preset threshold value into the same point set, wherein each point set is a set of all points contained in each obstacle;
calculating the length L of the barrier according to the maximum abscissa and the minimum abscissa, calculating the width W of the barrier according to the maximum ordinate and the minimum abscissa, and calculating the height H of the barrier according to the maximum ordinate and the minimum ordinate in the coordinates of all points of the point set to obtain three-dimensional data of the barrier;
calculating the coordinates of the obstacle laser radar according to a clustering algorithm, wherein the coordinates of the obstacle laser radar comprise the coordinates of a radar center point and the coordinates of radar edge points;
in the step S2, the calculating the obstacle GPS coordinates according to the obstacle lidar coordinates specifically includes:
the current GPS information of the vehicle is acquired, wherein the current GPS information comprises real-time GPS coordinates (lon, lat) and a course angle theta, and the obstacle laser radar coordinates (x, y) are substituted into the following formula:
x′＝xcosθ-ysinθ
y′＝xsinθ+ycosθ
a first coordinate (x ', y ') can be obtained, where x, y, x ' and y ' are all in m, and based on 1m= 0.00054054054' and the real-time GPS coordinate (lon, lat), it is possible to obtain:
lon'＝lon+x'×0.00054054054'
lat'＝lat+y'×0.00054054054'
thereby obtaining obstacle GPS coordinates (lon ', lat');
in this embodiment, according to the laser radar point cloud data, coordinates of an obstacle in the laser radar (i.e., obstacle laser radar coordinates) may be obtained according to a clustering algorithm and distances between points, including center point coordinates and edge point coordinates (i.e., radar center point coordinates and radar edge point coordinates), three-dimensional information of the obstacle is calculated, meanwhile, current GPS information of the vehicle is obtained, and real-time GPS coordinates, heading angle and obstacle laser radar coordinates are substituted into a formula and further calculated, so that obstacle GPS coordinates including GPS center point coordinates and GPS edge point coordinates of the obstacle obtained by substituting calculation of the radar center point coordinates may be obtained.
The step of judging whether obstacle detouring is needed according to the GPS coordinates and the three-dimensional data of the obstacle is specifically as follows:
judging whether a point (x_t, y_t) is located at a distance from the path track which is smaller than a preset dangerous distance and H is larger than a preset dangerous height according to the GPS coordinates of the obstacle, if so, carrying out obstacle detouring, otherwise, not carrying out obstacle detouring;
wherein x is min ≤x_t≤x max ，y min ≤y_t≤y max Wherein x is min 、x max 、y min And y max Respectively a minimum abscissa, a maximum abscissa, a minimum ordinate and a maximum ordinate in GPS edge point coordinates of the obstacle GPS coordinates;
in this embodiment, according to the GPS coordinates of the obstacle, we can determine whether there is a collision between the obstacle and the path track information, i.e. whether the obstacle is located in the dangerous range of the path track, i.e. within the preset dangerous distance, based on the GPS coordinates. In this embodiment, the preset dangerous distance is considered according to the vehicle width, and for example, the dangerous distance is 2 meters when the vehicle width is 4 meters. In other equivalent embodiments, the preset hazard distance may be increased or decreased appropriately based on the vehicle width and the vehicle length in view of safety.
Specifically, whether the abscissa of a point is between the maximum abscissa and the minimum abscissa can be judged according to the maximum and minimum abscissas in the coordinates of the GPS edge points, the ordinate is between the maximum ordinate and the minimum ordinate, the distance between the ordinate and the path track is smaller than the preset dangerous distance, the height of the obstacle (the height information H is obtained when the three-dimensional coordinates of the obstacle are calculated) is higher than the preset dangerous height, if yes, obstacle detouring is needed, otherwise, obstacle detouring is not needed, and the driver can drive along the original track. In this embodiment, the preset dangerous height is half of the height of the chassis of the vehicle, and in other equivalent embodiments, the preset dangerous height may be a preset height or may be appropriately increased or decreased according to the chassis height.
S3, when the vehicle is at a preset distance from the obstacle, generating an obstacle detouring route according to a preset obstacle detouring algorithm, and driving along the obstacle detouring route until the vehicle detours the obstacle and returns to the pre-acquired path track;
the path track includes a plurality of ordered target track points, as can be seen from fig. 4, the step S3 specifically includes:
adding a preset safety distance to the maximum abscissa of the edge points in the obstacle laser radar coordinatesThe obtained value is taken as the maximum value x of the obstacle detouring abscissa m ；
When the vehicle is at a preset distance from the obstacle, according to the radar center point coordinates and the radar edge point coordinates, taking a value interval (0, x) in the abscissa at a preset interval m ]The abscissa x1 is substituted into the following formula:
(x1-r) 2 +y1 2 ＝r*r
determining a plurality of turning radar track points (x 1, y 1), wherein r is a preset turning radius of the vehicle, and the preset distance is greater than the turning radius r of the vehicle plus a preset safety distance;
by the turning radar track point B (x m ，y b ) Determining a point C (x m ，y c ) Wherein y is c Adding a preset safety distance to the maximum ordinate of the radar edge point coordinates, and determining a point D at a position behind the obstacle, which is larger than a preset dangerous distance, so as to control the vehicle to travel to the turning radar track point B along the turning radar track point B and then along the turning radar track point B->C->D, the route is driven, and unmanned tracking driving is continued according to the current GPS information and the route track after the route reaches the point D;
in this embodiment, as shown in fig. 4, if the result of determining whether to need to perform obstacle avoidance is that obstacle avoidance is required, obstacle avoidance processing based on a laser radar is required. A point O (r, 0) determined based on the turning radius of the vehicle, that is, the preset turning radius r, and the maximum abscissa (obstacle detouring abscissa maximum value x m ) Thereby calculating a plurality of turning radar track points, determining a point B, C, D, controlling the vehicle to travel along the turning radar track points to a point B and along a rear path B->C->And D, driving can bypass the obstacle, and at the moment, the vehicle can be controlled to continuously enter and carry out unmanned tracking driving along the path track according to the GPS, namely, the target course angle can be obtained by the real-time GPS coordinates and the target course point according to the real-time GPS coordinates, the course angle and the target course point in the path track, and the vehicle is controlled to steer so as to adjust the current course angle to the target course angle, so that the vehicle is controlled to steer and drive along the path track.
Referring to fig. 2, a second embodiment of the present invention is as follows:
an unmanned obstacle detouring terminal 1 based on laser radar and GPS, comprising a processor 2, a memory 3 and a computer program stored on the memory 3 and executable on the processor 2, the steps of the above embodiment being implemented when the processor 2 executes the computer program.
In summary, according to the unmanned obstacle detouring method and terminal based on the laser radar and the GPS, the GPS coordinates and the three-dimensional data of the obstacle are determined according to the laser radar point cloud data, whether the obstacle damages a vehicle or not is judged according to the laser radar coordinates and the three-dimensional data, so that the automatic obstacle detouring of the unmanned vehicle is realized, and the position of the obstacle is judged, the height of the obstacle is comprehensively judged, whether the obstacle damages the vehicle or not is judged, unnecessary avoidance can be reduced, and the obstacle detouring efficiency is improved.
The foregoing description is only illustrative of the present invention and is not intended to limit the scope of the invention, and all equivalent changes made by the specification and drawings of the present invention, or direct or indirect application in the relevant art, are included in the scope of the present invention.

Claims (4)

1. An unmanned obstacle detouring method based on a laser radar and a GPS, which is characterized by comprising the following steps:
s1, starting a laser radar, and controlling a vehicle to run along a pre-acquired path track according to current GPS information;
s2, performing obstacle recognition according to point cloud data of a laser radar, acquiring three-dimensional data of an obstacle and obstacle laser radar coordinates, calculating an obstacle GPS (global positioning system) coordinate according to the obstacle laser radar coordinates, judging whether obstacle detouring is needed according to the obstacle GPS coordinate and the three-dimensional data, entering a step S3 if obstacle detouring is needed, and otherwise continuing to run along a path track;
in the step S2, the obstacle recognition is performed according to the point cloud data of the lidar, and the three-dimensional data of the obstacle and the coordinates of the lidar of the obstacle are specifically:
traversing point cloud data of a laser radar, calculating point distances between points, storing two points with the point distances smaller than a preset threshold value into the same point set, wherein each point set is a set of all points contained in each obstacle;
calculating the length L of the barrier according to the maximum abscissa and the minimum abscissa, calculating the width W of the barrier according to the maximum ordinate and the minimum abscissa, and calculating the height H of the barrier according to the maximum ordinate and the minimum ordinate in the coordinates of all points of the point set to obtain three-dimensional data of the barrier;
calculating the coordinates of the obstacle laser radar according to a clustering algorithm, wherein the coordinates of the obstacle laser radar comprise the coordinates of a radar center point and the coordinates of radar edge points;
in the step S2, calculating the obstacle GPS coordinates according to the obstacle lidar coordinates specifically includes:
the current GPS information of the vehicle is acquired, wherein the current GPS information comprises real-time GPS coordinates (lon, lat) and a course angle theta, and the obstacle laser radar coordinates (x, y) are substituted into the following formula:
x′＝xcosθ-ysinθ
y′＝xsinθ+ycosθ
a first coordinate (x ', y ') can be obtained, where x, y, x ' and y ' are all in m, and based on 1m= 0.00054054054' and the real-time GPS coordinate (lon, lat), it is possible to obtain:
lon'＝lon+x'×0.00054054054'
lat'＝lat+y'×0.00054054054'
thereby obtaining obstacle GPS coordinates (lon ', lat');
s3, when the vehicle is at a preset distance from the obstacle, generating an obstacle detouring route according to a preset obstacle detouring algorithm, and driving along the obstacle detouring route until the vehicle detours the obstacle and returns to the pre-acquired path track;
the path track includes a plurality of ordered target track points, and the step S3 specifically includes:
taking a value obtained by adding a preset safety interval to the maximum abscissa of the edge points in the obstacle laser radar coordinates as a maximum value x of obstacle-detouring abscissas m ；
When the vehicle is at a preset distance from the obstacle, according to the radar center point coordinates and the radar edge point coordinates, taking a value interval (0, x) in the abscissa at a preset interval m ]The abscissa x1 is substituted into the following formula:
(x1-r) 2 +y1 2 ＝r*r
determining a plurality of turning radar track points (x 1, y 1), wherein r is a preset turning radius of the vehicle, and the preset distance is greater than the turning radius r of the vehicle plus a preset safety distance;
by the turning radar track point B (x m ，y b ) Determining a point C (x m ，y c ) Wherein y is c Adding a preset safety distance to the maximum ordinate of the radar edge point coordinates, and determining a point D at a position behind the obstacle, which is larger than a preset dangerous distance, so as to control the vehicle to travel to the turning radar track point B along the turning radar track point B and then along the turning radar track point B->C->And D, continuing unmanned tracking driving according to the current GPS information and the path track after the point D is reached.
2. The unmanned obstacle detouring method based on the laser radar and the GPS according to claim 1, wherein the judging whether the obstacle detouring is needed according to the GPS coordinates and the three-dimensional data is specifically as follows:
judging whether a point (x_t, y_t) is located at a distance from the path track which is smaller than a preset dangerous distance and H is larger than a preset dangerous height according to the GPS coordinates of the obstacle, if so, carrying out obstacle detouring, otherwise, not carrying out obstacle detouring;
wherein x is min ≤x_t≤x max ，y min ≤y_t≤y max Wherein x is min 、x max 、y min And y max GPS edge point coordinates respectively being the obstacle GPS coordinatesThe minimum abscissa, the maximum abscissa, the minimum ordinate, and the maximum ordinate of (a).
3. An unmanned obstacle detouring terminal based on a laser radar and a GPS, which is characterized by comprising a processor, a memory and a computer program stored on the memory and capable of running on the processor, wherein the following steps are realized when the processor executes the computer program:
s1, starting a laser radar, and controlling a vehicle to run along a pre-acquired path track according to current GPS information;
s2, performing obstacle recognition according to point cloud data of a laser radar, acquiring three-dimensional data of an obstacle and obstacle laser radar coordinates, calculating an obstacle GPS (global positioning system) coordinate according to the obstacle laser radar coordinates, judging whether obstacle detouring is needed according to the obstacle GPS coordinate and the three-dimensional data, entering a step S3 if obstacle detouring is needed, and otherwise continuing to run along a path track;
in the step S2, the obstacle recognition is performed according to the point cloud data of the lidar, and the three-dimensional data of the obstacle and the coordinates of the lidar of the obstacle are specifically:
traversing point cloud data of a laser radar, calculating point distances between points, storing two points with the point distances smaller than a preset threshold value into the same point set, wherein each point set is a set of all points contained in each obstacle;
calculating the length L of the barrier according to the maximum abscissa and the minimum abscissa, calculating the width W of the barrier according to the maximum ordinate and the minimum abscissa, and calculating the height H of the barrier according to the maximum ordinate and the minimum ordinate in the coordinates of all points of the point set to obtain three-dimensional data of the barrier;
calculating the coordinates of the obstacle laser radar according to a clustering algorithm, wherein the coordinates of the obstacle laser radar comprise the coordinates of a radar center point and the coordinates of radar edge points;
in the step S2, calculating the obstacle GPS coordinates according to the obstacle lidar coordinates specifically includes:
the current GPS information of the vehicle is acquired, wherein the current GPS information comprises real-time GPS coordinates (lon, lat) and a course angle theta, and the obstacle laser radar coordinates (x, y) are substituted into the following formula:
x′＝xcosθ-ysinθ
y′＝xsinθ+ycosθ
a first coordinate (x ', y ') can be obtained, where x, y, x ' and y ' are all in m, and based on 1m= 0.00054054054' and the real-time GPS coordinate (lon, lat), it is possible to obtain:
lon'＝lon+x'×0.00054054054'
lat'＝lat+y'×0.00054054054'
thereby obtaining obstacle GPS coordinates (lon ', lat');
s3, when the vehicle is at a preset distance from the obstacle, generating an obstacle detouring route according to a preset obstacle detouring algorithm, and driving along the obstacle detouring route until the vehicle detours the obstacle and returns to the pre-acquired path track;
the path track includes a plurality of ordered target track points, and the step S3 specifically includes:
taking a value obtained by adding a preset safety interval to the maximum abscissa of the edge points in the obstacle laser radar coordinates as a maximum value x of obstacle-detouring abscissas m ；
When the vehicle is at a preset distance from the obstacle, according to the radar center point coordinates and the radar edge point coordinates, taking a value interval (0, x) in the abscissa at a preset interval m ]The abscissa x1 is substituted into the following formula:
(x1-r) 2 +y1 2 ＝r*r
determining a plurality of turning radar track points (x 1, y 1), wherein r is a preset turning radius of the vehicle, and the preset distance is greater than the turning radius r of the vehicle plus a preset safety distance;
by the turning radar track point B (x m ，y b ) Determining a point C (x m ，y c ) Wherein y is c Adding a preset safety distance to the maximum ordinate of the radar edge point coordinates, and locating behind the obstacle at a position greater than a preset dangerous distanceA point D is determined so as to control the vehicle to travel along the turning radar track point to the turning radar track point B and then along the line B->C->And D, continuing unmanned tracking driving according to the current GPS information and the path track after the point D is reached.
4. The unmanned obstacle detouring terminal based on the laser radar and the GPS according to claim 3, wherein the judging whether the obstacle detouring is needed according to the GPS coordinates and the three-dimensional data is specifically as follows:
judging whether a point (x_t, y_t) is located at a distance from the path track which is smaller than a preset dangerous distance and H is larger than a preset dangerous height according to the GPS coordinates of the obstacle, if so, carrying out obstacle detouring, otherwise, not carrying out obstacle detouring;
wherein x is min ≤x_t≤x max ，y min ≤y_t≤y max Wherein x is min 、x max 、y min And y max And the minimum abscissa, the maximum abscissa, the minimum ordinate and the maximum ordinate of the GPS edge point coordinates of the obstacle GPS coordinates are respectively.
CN202110562594.5A 2021-05-24 2021-05-24 Unmanned obstacle detouring method and terminal based on laser radar and GPS Active CN113296118B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110562594.5A CN113296118B (en) 2021-05-24 2021-05-24 Unmanned obstacle detouring method and terminal based on laser radar and GPS

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110562594.5A CN113296118B (en) 2021-05-24 2021-05-24 Unmanned obstacle detouring method and terminal based on laser radar and GPS

Publications (2)

Publication Number Publication Date
CN113296118A CN113296118A (en) 2021-08-24
CN113296118B true CN113296118B (en) 2023-11-24

Family

ID=77324014

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110562594.5A Active CN113296118B (en) 2021-05-24 2021-05-24 Unmanned obstacle detouring method and terminal based on laser radar and GPS

Country Status (1)

Country Link
CN (1) CN113296118B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114415664A (en) * 2021-12-16 2022-04-29 北京航天测控技术有限公司 Robot navigation method and device
CN114407929B (en) * 2022-01-29 2023-12-12 上海木蚁机器人科技有限公司 Unmanned obstacle detouring processing method and device, electronic equipment and storage medium
CN115390571B (en) * 2022-10-27 2023-03-24 杭州蓝芯科技有限公司 Obstacle-detouring driving method and mobile robot

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102275579A (en) * 2004-03-03 2011-12-14 日产自动车株式会社 Device for assisting drive of vehicle
CN105549597A (en) * 2016-02-04 2016-05-04 同济大学 Unmanned vehicle dynamic path programming method based on environment uncertainty
CN105549044A (en) * 2015-12-17 2016-05-04 中国电子科技集团公司第三十八研究所 Combined positioning device and method of GNSS (Global Navigation Satellite System) based on data fusion
CN109829386A (en) * 2019-01-04 2019-05-31 清华大学 Intelligent vehicle based on Multi-source Information Fusion can traffic areas detection method
CN110168559A (en) * 2017-12-11 2019-08-23 北京嘀嘀无限科技发展有限公司 For identification with positioning vehicle periphery object system and method
CN110389589A (en) * 2019-07-26 2019-10-29 阿尔法巴人工智能(深圳)有限公司 Intelligent driving vehicle obstacle-avoidance system and method
CN110525449A (en) * 2019-09-20 2019-12-03 昆山聚创新能源科技有限公司 The method for security protection and system of vehicle
CN111071249A (en) * 2019-12-16 2020-04-28 南京航空航天大学 Unmanned vehicle emergency steering obstacle avoidance method based on obstacle motion prediction
CN111284485A (en) * 2019-10-10 2020-06-16 中国第一汽车股份有限公司 Method and device for predicting driving behavior of obstacle vehicle, vehicle and storage medium
CN111309032A (en) * 2020-04-08 2020-06-19 江苏盛海智能科技有限公司 Autonomous obstacle avoidance method and control end of unmanned vehicle
WO2020147485A1 (en) * 2019-01-15 2020-07-23 北京百度网讯科技有限公司 Information processing method, system and equipment, and computer storage medium

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6529855B2 (en) * 2015-08-07 2019-06-12 株式会社Ｉｈｉエアロスペース Moving object and its obstacle detection method
TWI680898B (en) * 2018-11-23 2020-01-01 財團法人車輛研究測試中心 Light reaching detection device and method for close obstacles

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102275579A (en) * 2004-03-03 2011-12-14 日产自动车株式会社 Device for assisting drive of vehicle
CN105549044A (en) * 2015-12-17 2016-05-04 中国电子科技集团公司第三十八研究所 Combined positioning device and method of GNSS (Global Navigation Satellite System) based on data fusion
CN105549597A (en) * 2016-02-04 2016-05-04 同济大学 Unmanned vehicle dynamic path programming method based on environment uncertainty
CN110168559A (en) * 2017-12-11 2019-08-23 北京嘀嘀无限科技发展有限公司 For identification with positioning vehicle periphery object system and method
CN109829386A (en) * 2019-01-04 2019-05-31 清华大学 Intelligent vehicle based on Multi-source Information Fusion can traffic areas detection method
WO2020147485A1 (en) * 2019-01-15 2020-07-23 北京百度网讯科技有限公司 Information processing method, system and equipment, and computer storage medium
CN110389589A (en) * 2019-07-26 2019-10-29 阿尔法巴人工智能(深圳)有限公司 Intelligent driving vehicle obstacle-avoidance system and method
CN110525449A (en) * 2019-09-20 2019-12-03 昆山聚创新能源科技有限公司 The method for security protection and system of vehicle
CN111284485A (en) * 2019-10-10 2020-06-16 中国第一汽车股份有限公司 Method and device for predicting driving behavior of obstacle vehicle, vehicle and storage medium
CN111071249A (en) * 2019-12-16 2020-04-28 南京航空航天大学 Unmanned vehicle emergency steering obstacle avoidance method based on obstacle motion prediction
CN111309032A (en) * 2020-04-08 2020-06-19 江苏盛海智能科技有限公司 Autonomous obstacle avoidance method and control end of unmanned vehicle

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Radar Sensing for Intelligent Vihicles in Urban Environments;Giulio Reina 等;Sensors;第15卷(第6期);14661-14678 *
智能车辆车道保持系统中避障路径规划;汪明磊;合肥工业大学学报(自然科学版);第37卷(第2期);129-133 *

Also Published As

Publication number Publication date
CN113296118A (en) 2021-08-24

Similar Documents

Publication Publication Date Title
CN113296118B (en) Unmanned obstacle detouring method and terminal based on laser radar and GPS
CN113296119B (en) Unmanned obstacle avoidance driving method and terminal based on laser radar and UWB array
US10988139B2 (en) Vehicle position control method and device vehicle position control device for correcting position in drive-assisted vehicle
CN107544514B (en) Robot obstacle avoiding method and device, storage medium and robot
CN111746504B (en) Memory parking method, system, device and computer readable storage medium
JP7193656B2 (en) Control unit and method for recognizing intruding or exiting vehicles
CN110834630A (en) Vehicle driving control method and device, vehicle and storage medium
CN112721952B (en) Complex road section driving control method and device for L3-grade automatic driving automobile, automobile and computer storage medium
JP2010238132A (en) Vehicle controller, vehicle and vehicle control program
CN113353103A (en) Method, device, equipment and medium for controlling speed of curve vehicle
CN111746518A (en) Vehicle parking-out control method and device
CN111445723A (en) Vehicle path identification
CN112099488A (en) Narrow-passage passing method and device for mobile robot, mower and storage medium
EP4335710A1 (en) Traveling path boundary determination method and device, vehicle, storage medium, and terminal
CN112099497A (en) Obstacle avoidance control method and device, vehicle and medium
CN112519764A (en) Parking space angle identification method and automatic parking method
JP6720751B2 (en) Parking assistance method and parking assistance device
CN113353064B (en) Automatic parking driving control method
CN114084132A (en) Control method and device and terminal equipment
JP2018083480A (en) Parking support method and parking support device
CN114103957B (en) Lane change control method, apparatus, electronic device and storage medium
CN115056765A (en) Path planning method and device for autonomous parking in vertical and inclined parking spaces
CN113885525A (en) Path planning method and system for automatically driving vehicle to get rid of trouble, vehicle and storage medium
CN113625709A (en) Obstacle avoidance method for unmanned surface vehicle
CN111176285B (en) Method and device for planning travel path, vehicle and readable storage medium

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20231031

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

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

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

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

GR01 Patent grant
GR01 Patent grant