CN114995374A - Obstacle bypassing method and terminal for unmanned vehicle - Google Patents
Obstacle bypassing method and terminal for unmanned vehicle Download PDFInfo
- Publication number
- CN114995374A CN114995374A CN202210398036.4A CN202210398036A CN114995374A CN 114995374 A CN114995374 A CN 114995374A CN 202210398036 A CN202210398036 A CN 202210398036A CN 114995374 A CN114995374 A CN 114995374A
- Authority
- CN
- China
- Prior art keywords
- point
- detour
- path
- obstacle
- bypassing
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 230000004888 barrier function Effects 0.000 claims description 35
- 239000013589 supplement Substances 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 8
- 230000009286 beneficial effect Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000004804 winding Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an obstacle detouring method and a terminal of an unmanned vehicle, wherein an obstacle entry point and an obstacle exit point are calculated based on the circle center and the detouring radius of an obstacle, and a first detouring path is calculated; the method comprises the steps of obtaining a connecting line corresponding to an obstacle entry point and an obstacle exit point in a first detour path and detour points on different sides of the circle center of an obstacle, calculating first included angles between the detour points on the different sides and the obstacle entry point and the obstacle exit point, determining a second detour path according to the size relation of the first included angles and a preset angle, and ensuring that an unmanned vehicle smoothly enters the detour path. And determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour, so that the situation of sharp turning when the unmanned vehicle enters the detour route is avoided, and the unmanned vehicle is further ensured to smoothly enter the detour route.
Description
Technical Field
The invention relates to the technical field of unmanned vehicle control, in particular to an obstacle bypassing method and a terminal of an unmanned vehicle.
Background
At present, when a planned tracking route is set for an unmanned vehicle on a command platform, the terrain is not always flat and wide and can move straightly at will, and obstacles can be touched on the tracking route at some time. An operator marks certain obstacles on the finger control platform into a circular obstacle with a certain point as the center of a circle and a fixed radius. When planning a route, route points need to bypass the circular obstacles, and the unmanned vehicle is prevented from touching the obstacles in the driving process. However, in the prior art, the detouring route of the round obstacle is not smooth enough in the detouring process, and the situation of turning over due to sharp turning can occur.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: provided are an obstacle detouring method and a terminal of an unmanned vehicle, which can realize smooth detouring of the unmanned vehicle.
In order to solve the technical problems, the invention adopts the technical scheme that:
an obstacle detouring method of an unmanned vehicle, comprising the steps of:
calculating an obstacle entry point and an obstacle exit point in a planned route based on the circle center and the detour radius of an obstacle, and drawing a first detour path between the obstacle entry point and the obstacle exit point;
acquiring a detouring point at different sides of the circle center of the barrier and a connecting line corresponding to a barrier entry point and a barrier exit point in a first detouring path, calculating a first included angle between the detouring point at different sides and the barrier entry point and the barrier exit point, and determining a second detouring path according to the size relationship between the first included angle and a preset angle;
determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour path in the paths before and after detour;
and splicing the starting point of the planned route, the pre-detour starting point, the second detour path, the pre-detour end point and the end point of the planned route.
In order to solve the technical problem, the invention adopts another technical scheme as follows:
an obstacle detour terminal for an unmanned vehicle, comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the following steps when executing the computer program:
calculating an obstacle entry point and an obstacle exit point in a planned route based on the circle center and the detour radius of an obstacle, and drawing a first detour path between the obstacle entry point and the obstacle exit point;
acquiring a detour point of a first detour path on different sides of the circle center of the barrier relative to a connecting line of a barrier entry point and a barrier exit point and calculating a first included angle between the detour point on the different sides and the barrier entry point and the barrier exit point, and determining a second detour path according to the size relation between the first included angle and a preset angle;
determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour;
and splicing the starting point of the planned route, the pre-detour starting point, the second detour path, the pre-detour end point and the end point of the planned route.
The invention has the beneficial effects that: calculating an obstacle entry point and an obstacle exit point based on the circle center and the detour radius of the obstacle, and calculating a first detour path; the method comprises the steps of obtaining a connecting line corresponding to an obstacle entry point and an obstacle exit point in a first detour path and detour points on different sides of the circle center of an obstacle, calculating first included angles between the detour points on the different sides and the obstacle entry point and the obstacle exit point, determining a second detour path according to the size relation of the first included angles and a preset angle, and ensuring that an unmanned vehicle smoothly enters the detour path. And determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour, so that the situation of sharp turning when the unmanned vehicle enters the detour route is avoided, and the unmanned vehicle is further ensured to smoothly enter the detour route.
Drawings
Fig. 1 is a flowchart of an obstacle detouring method of an unmanned vehicle according to an embodiment of the present invention;
fig. 2 is a structural view of an obstacle detouring terminal of an unmanned vehicle according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of an unmanned vehicle detouring in accordance with an embodiment of the present invention;
description of reference numerals:
1. an obstacle detouring terminal of an unmanned vehicle; 2. a memory; 3. a processor.
Detailed Description
In order to explain technical contents, achieved objects, and effects of the present invention in detail, the following description is made with reference to the accompanying drawings in combination with the embodiments.
Referring to fig. 1 and 3, an embodiment of the present invention provides an obstacle detouring method for an unmanned vehicle, including:
calculating an obstacle entry point and an obstacle exit point in a planned route based on the circle center and the detour radius of an obstacle, and drawing a first detour path between the obstacle entry point and the obstacle exit point;
acquiring a detouring point at different sides of the circle center of the barrier and a connecting line corresponding to a barrier entry point and a barrier exit point in a first detouring path, calculating a first included angle between the detouring point at different sides and the barrier entry point and the barrier exit point, and determining a second detouring path according to the size relationship between the first included angle and a preset angle;
determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour;
and splicing the starting point of the planned route, the pre-detour starting point, the second detour path, the pre-detour end point and the end point of the planned route.
From the above description, the beneficial effects of the present invention are: calculating an obstacle entry point and an obstacle exit point based on the circle center and the detour radius of the obstacle, and calculating a first detour path; the method comprises the steps of obtaining a connecting line corresponding to an obstacle entry point and an obstacle exit point in a first detour path and detour points on different sides of the circle center of an obstacle, calculating first included angles between the detour points on the different sides and the obstacle entry point and the obstacle exit point, determining a second detour path according to the size relation between the first included angles and a preset angle, and ensuring that an unmanned vehicle can smoothly enter the detour path. And determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour, so that the situation of sharp turning when the unmanned vehicle enters the detour route is avoided, and the unmanned vehicle is further ensured to smoothly enter the detour route.
Further, the calculating an obstacle entry point and an obstacle exit point in the planned route based on the center of the circle and the radius of the detour of the obstacle comprises:
calculating a bypassing radius according to the radius of the obstacle and a preset safe distance;
and sequentially traversing each path point in the planned route, and judging whether the distance between each path point and the center of the obstacle is equal to the bypassing radius, if so, taking the path point close to the starting point of the planned route as an obstacle entry point, and taking the path point close to the end point of the planned route as an obstacle exit point.
As can be seen from the above description, the obstacle entry point and the obstacle exit point are determined according to the distance between each path point in the planned route and the center of the obstacle, so as to facilitate the subsequent calculation of the detour path and the pre-detour path.
Further, determining the second detour path according to the magnitude relation between the first included angle and the preset angle includes:
calculating preset angles between the circle center of the obstacle and the obstacle entry point and between the circle center of the obstacle and the obstacle exit point;
and judging whether the first included angle is smaller than or equal to the preset angle, if so, adding the bypassing points to a second bypassing path, and if not, adding the bypassing points of different sides to the second bypassing path when all the first included angles are larger than the preset angle.
According to the description, the angle of the unmanned vehicle entering the second detour path can be increased by judging whether the first included angle is smaller than or equal to the preset angle, and when the first included angle is larger than the preset angle, the current path can be smoothly detoured, so that the detour path can be flexibly acquired.
Further, the determining the pre-detour starting point and the pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after the detour includes:
judging whether a second included angle between a path point of the planned path in the path before detour and a starting point of a second detour path is equal to a safety included angle or not, if so, the path point is a pre-detour starting point, and if not, when all the second included angles are not equal to preset angles, the second detour path starting point is taken as a pre-detour starting point;
judging whether a second included angle between a path point of the planned path in the bypassed path and a second bypassing path terminal point is equal to a safety included angle or not, if so, the path point is a pre-bypassing terminal point, and if not, when all the second included angles are not equal to preset angles, taking the second bypassing path terminal point as a pre-bypassing terminal point.
According to the above description, the pre-detour starting point and the pre-detour end point can be obtained by judging the second included angle and the safety included angle, and the unmanned vehicle can smoothly enter the second detour path without deviating the planned path in a large amount through the pre-detour starting point and the pre-detour end point.
Further, the splicing the starting point of the planned route, the pre-detour starting point, the second detour path, the pre-detour end point and the end point of the planned route includes:
taking a path from a starting point to the pre-detour starting point in the planned route as a first planned path, and taking a path from a terminal point to the pre-detour terminal point in the planned route as a second planned path;
performing linear point supplement on the pre-detour starting point to the starting point of a second detour path to obtain a first pre-detour path, and performing linear point supplement on the end point of the second detour path to the pre-detour end point to obtain a second pre-detour path;
and splicing the first planning path, the first pre-bypassing path, the second pre-bypassing path and the second planning path in sequence.
As can be seen from the above description, performing linear point compensation on the pre-detour starting point to the starting point of the second detour path, and performing linear point compensation on the second detour path from the end point to the pre-detour end point can obtain the pre-detour path, facilitate path splicing, and obtain the complete obstacle detour path of the unmanned vehicle.
Referring to fig. 2, another embodiment of the present invention provides an obstacle detouring terminal for an unmanned vehicle, including a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor implements the following steps when executing the computer program:
calculating an obstacle entry point and an obstacle exit point in a planned route based on the circle center and the detour radius of an obstacle, and drawing a first detour path between the obstacle entry point and the obstacle exit point;
acquiring a detouring point at different sides of the circle center of the barrier and a connecting line corresponding to a barrier entry point and a barrier exit point in a first detouring path, calculating a first included angle between the detouring point at different sides and the barrier entry point and the barrier exit point, and determining a second detouring path according to the size relationship between the first included angle and a preset angle;
determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour;
and splicing the starting point of the planned route, the pre-detour starting point, the second detour path, the pre-detour end point and the end point of the planned route.
From the above description, the beneficial effects of the present invention are: calculating an obstacle entry point and an obstacle exit point based on the circle center and the detour radius of the obstacle, and calculating a first detour path; the method comprises the steps of obtaining a connecting line corresponding to an obstacle entry point and an obstacle exit point in a first detour path and detour points on different sides of the circle center of an obstacle, calculating first included angles between the detour points on the different sides and the obstacle entry point and the obstacle exit point, determining a second detour path according to the size relation of the first included angles and a preset angle, and ensuring that an unmanned vehicle smoothly enters the detour path. And determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour, so that the situation of sharp turning when the unmanned vehicle enters the detour route is avoided, and the unmanned vehicle is further ensured to smoothly enter the detour route.
Further, the calculating an obstacle entry point and an obstacle exit point in the planned route based on the center of the circle and the radius of the detour of the obstacle comprises:
calculating a bypassing radius according to the radius of the obstacle and a preset safe distance;
and sequentially traversing each path point in the planned route, judging whether the distance between each path point and the center of the obstacle is equal to the detour radius, if so, taking the path point close to the starting point of the planned route as an obstacle entry point, and taking the path point close to the end point of the planned route as an obstacle exit point.
As can be seen from the above description, the obstacle entry point and the obstacle exit point are determined according to the distance between each path point in the planned route and the center of the obstacle, so as to facilitate the subsequent calculation of the detour path and the pre-detour path.
Further, determining the second detour path according to the magnitude relation between the first included angle and the preset angle includes:
calculating preset angles between the circle center of the obstacle and the obstacle entry point and between the circle center of the obstacle and the obstacle exit point;
and judging whether the first included angle is smaller than or equal to the preset angle, if so, adding the bypassing points to a second bypassing path, and if not, adding the bypassing points of different sides to the second bypassing path when all the first included angles are larger than the preset angle.
According to the description, the angle of the unmanned vehicle entering the second detour path can be increased by judging whether the first included angle is smaller than or equal to the preset angle, and when the first included angle is larger than the preset angle, the current path can be smoothly detoured, so that the detour path can be flexibly acquired.
Further, the determining the pre-detour starting point and the pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after the detour includes:
judging whether a second included angle between a path point of the planned path in the path before detour and a starting point of a second detour path is equal to a safety included angle or not, if so, the path point is a pre-detour starting point, and if not, when all the second included angles are not equal to preset angles, the second detour path starting point is taken as a pre-detour starting point;
judging whether a second included angle between a path point of the planned path in the bypassed path and a second bypassing path terminal point is equal to a safety included angle or not, if so, the path point is a pre-bypassing terminal point, and if not, when all the second included angles are not equal to preset angles, taking the second bypassing path terminal point as a pre-bypassing terminal point.
According to the above description, the pre-detour starting point and the pre-detour end point can be obtained by judging the second included angle and the safety included angle, and the unmanned vehicle can smoothly enter the second detour path without deviating the planned path in a large amount through the pre-detour starting point and the pre-detour end point.
Further, the splicing the starting point of the planned route, the pre-detour starting point, the second detour path, the pre-detour end point and the end point of the planned route includes:
taking a path from a starting point to the pre-detour starting point in the planned route as a first planned path, and taking a path from a terminal point to the pre-detour terminal point in the planned route as a second planned path;
performing linear point supplement on the pre-detour starting point to the starting point of a second detour path to obtain a first pre-detour path, and performing linear point supplement on the end point of the second detour path to the pre-detour end point to obtain a second pre-detour path;
and splicing the first planning path, the first pre-bypassing path, the second pre-bypassing path and the second planning path in sequence.
As can be seen from the above description, the detour route can be obtained by performing linear point compensation on the detour starting point to the starting point of the second detour route and performing linear point compensation on the detour end point to the detour end point of the second detour route, so that the route splicing is facilitated, and the complete obstacle detour route of the unmanned vehicle is obtained.
The obstacle detouring method and the terminal of the unmanned vehicle are suitable for detouring an obstacle when the unmanned vehicle has a planned path, and are described in the following through specific implementation modes:
example one
Referring to fig. 1, an obstacle detouring method for an unmanned vehicle includes the steps of:
s1, calculating an obstacle entry point and an obstacle exit point in the planned route based on the circle center and the detour radius of the obstacle, and drawing a first detour path between the obstacle entry point and the obstacle exit point.
And S11, calculating a detour radius according to the radius of the obstacle and a preset safe distance.
Specifically, when the obstacle is detoured, the obstacle cannot be detoured until the obstacle is touched, and a safety distance rx needs to be set to ensure that the unmanned vehicle does not collide with the obstacle when detouring, so that the actual detouring radius of the unmanned vehicle should be rdis ═ r + rx, where r is the radius of the obstacle, and in some embodiments, may be the length of the farthest point between the center of the obstacle and the edge of the obstacle.
And S12, sequentially traversing each path point in the planned route, and judging whether the distance between each path point and the center of the obstacle is equal to the detour radius, if so, taking the path point close to the starting point of the planned route as an obstacle entry point, and taking the path point close to the end point of the planned route as an obstacle exit point.
Specifically, each path point on the planned route is traversed, and the distance is calculated according to the longitude and latitude of each path point and the longitude and latitude of the center of the obstacle circle. And judging whether the distance between the path point and the circle center is equal to rdis or not, if so, taking the path point close to the starting point of the planned route as an obstacle entry point, and taking the path point close to the end point of the planned route as an obstacle exit point.
S2, acquiring a detour point of the first detour path, which is on different sides of the circle center of the barrier and corresponds to a connecting line of the barrier entry point and the barrier exit point, calculating a first included angle between the detour point on the different sides and the barrier entry point and the barrier exit point, and determining a second detour path according to the size relation between the first included angle and a preset angle.
And S21, calculating preset angles between the center of the obstacle and the obstacle entry point and the obstacle exit point.
Specifically, three points of a point I, a circle center and a point O are used for judging whether the circle center is on the left side or the right side (isLeft) of an IO line through a space coordinate vector function in the system, and an included angle of the three points is obtained.
And S22, judging whether the first included angle is smaller than or equal to the preset angle, if so, adding the detour points into a second detour path, and if not, adding the detour points on different sides into the second detour path when all the first included angles are larger than the preset angle.
Specifically, a point tempP in the first detour path is traversed, and whether the detour curve point is on the left side or the right side of the IO line (tempIsLeft) is obtained by using three points, i.e., the point I, the detour curve point (tempP) and the point O.
The included angle tempAngle of these three points and the distance dis of the detour curve point to point I are obtained and if tempIsLeft is opposite to isLeft and tempAngle is less than or equal to angle, this point is added to the second detour path r 2.
When all the first included angles are larger than the preset angle, the detour points of different sides are added into the second detour path r 2.
In some embodiments, the path points in the second detour path are sorted according to dis.
And S3, determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour.
S31, judging whether a second included angle between a path point of the planned path in the path before detour and a starting point of a second detour path is equal to a safety included angle or not, if so, the path point is a pre-detour starting point, and if not, when all the second included angles are not equal to preset angles, the second detour path starting point is used as the pre-detour starting point.
Specifically, a subscript (Iindex) of the point I in the whole planned route is obtained, and starting with 0 and ending with Iindex, a path point set is extracted from the whole planned route, that is, the planned path point set before detour is entered.
In order to avoid the situation that the angle of the planned route entering the second detour path is too narrow, the unmanned vehicle is turned over due to sharp turning in the driving process, and therefore the driving-in detour angle needs to be judged for the planned route point set before detour.
Traversing the planned path set before the detour in a mode from the end to the head:
and (3) regarding the starting point of the second detour path as r20 point, regarding the point traversed in the planned path before detour as p point, and regarding the last point of the p point in the planned path as a grandP point, then obtaining the included angle degree of the r20, the p point and the grandP point.
If the degree is less than a specific angle (e.g. 150 °), the point is skipped to judge the next point until the point a satisfying the condition is found, and the index i of the point is recorded.
If both degree are larger than a specific angle (e.g. 150 °), the second detour path start point is taken as the pre-detour start point a.
And S32, judging whether a second included angle between a path point of the planned path in the bypassed path and a second bypassing path end point is equal to a safety included angle or not, if so, the path point is a pre-bypassing end point.
Specifically, subscripts (Oindex) of the points O in the whole planned route are obtained, and with the Oindex as a start point and an end point as an end point, a path point set, that is, a bypassed planned route point set, is extracted from the whole planned route.
In order to avoid the situation that the angle of the second detour path entering the detoured planned route is too narrow after the second detour path comes out, and the unmanned vehicle turns over due to sharp turning in the driving process, the driving-in detour angle of r3 needs to be judged.
Traversing the set of the planned paths after the detour:
and (3) regarding the end point of the second detour path as a point r2i, regarding the point traversed in the detoured planned path as a point p, and regarding the last point of the point p in the planned path as a grandP point, so that the included angle degree of the three points r2i, p and grandP can be obtained.
If the degree is smaller than a specific angle (e.g. 150 °), the point is skipped to judge the next point until the point c satisfying the condition is found, and the index j of the point is recorded.
If both degrees are larger than a specific angle (e.g., 150 °), the second detour path end point is taken as the pre-detour end point c.
And S4, splicing the starting point of the planned route, the pre-detour starting point, the second detour path, the pre-detour end point and the end point of the planned route.
And S41, taking a path from the starting point to the pre-detour starting point in the planned route as a first planned path, and taking a path from the end point to the pre-detour end point in the planned route as a second planned path.
Specifically, starting with 0 and ending with i, a set of waypoints is intercepted from the planned path before detour, and is assigned to r 1.
And taking j as a start point and taking an end point as an end point, intercepting a path point set from the planned path after the detour, and assigning the path point set to r 3.
And S42, performing linear point supplement on the pre-detour starting point to the starting point of the second detour path to obtain a first pre-detour path, and performing linear point supplement on the end point of the second detour path to the pre-detour end point to obtain a second pre-detour path.
Specifically, the last point of r1 and the first point of r2 are subjected to point complementation to obtain a path point set r 12. And (5) performing point interpolation between the last point of r2 and the first point of r3 to obtain a path point set r 23.
And S43, sequentially splicing the first planned path, the first pre-winding path, the second pre-winding path and the second planned path.
The five sets are spliced together in the order r1, r12, r2, r23, r3 to obtain the final modified back-end planned route.
Example two
Referring to fig. 3, the present embodiment provides an application scenario in which the planned path within the range of the obstacle is not a straight line, specifically:
the mode of determining the second detour path is specifically as follows:
And 2, traversing the point tempP in the first detour path, and acquiring whether the detour curve point is on the left side or the right side (tempIsLeft) of the IO line by using the three points of the I point, the detour curve point (tempP) and the O point.
And 3, acquiring an included angle tempAngle of the three points and a distance dis from a detour curve point to the point I, and if tempIsLeft is opposite to isLeft and tempAngle is less than or equal to angle, adding the point into a second detour path r 2.
The mode for determining the pre-detour starting point and the pre-detour end point is specifically as follows:
and obtaining the subscript (Iindex) of the I point in the whole planned route, and taking 0 as the start and Iindex as the end to intercept a path point set from the whole planned route, namely the planned path point set before the detour.
Traversing the planned path set before the detour in a mode from the end to the head:
and (3) regarding the starting point of the second detour path as r20 point, regarding the point traversed in the planned path before detour as p point, and regarding the last point of the p point in the planned path as a grandP point, then obtaining the included angle degree of the r20, the p point and the grandP point.
If the degree is less than a specific included angle (for example 150 degrees), skipping the point to judge the next point until finding the point a meeting the condition, recording the subscript i of the point, and ending the loop.
And judging whether a second included angle between a path point of the planned path in the bypassed path and a second bypassing path end point is equal to a safety included angle or not, if so, the path point is a pre-bypassing end point.
Similarly, the pre-detour end point is calculated in the above manner.
EXAMPLE III
Referring to fig. 2, an obstacle detour terminal 1 for an unmanned vehicle includes a memory 2, a processor 3, and a computer program stored in the memory and executable on the processor, and the processor executes the computer program to implement the steps of the obstacle detour method for an unmanned vehicle according to one or two embodiments.
In summary, according to the obstacle detouring method and the terminal for the unmanned vehicle provided by the invention, the obstacle entry point and the obstacle exit point are calculated based on the circle center and the detouring radius of the obstacle, and the first detouring path is calculated; the method comprises the steps of obtaining a connecting line corresponding to an obstacle entry point and an obstacle exit point in a first detour path and detour points on different sides of the circle center of an obstacle, calculating first included angles between the detour points on the different sides and the obstacle entry point and the obstacle exit point, determining a second detour path according to the size relation of the first included angles and a preset angle, and ensuring that an unmanned vehicle smoothly enters the detour path. And determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour, so that the situation of sharp turning when the unmanned vehicle enters the detour route is avoided, and the unmanned vehicle is further ensured to smoothly enter the detour route.
The above description is only an embodiment of the present invention, and is not intended to limit the scope of the present invention, and all equivalent modifications made by the present invention and the contents of the accompanying drawings, which are directly or indirectly applied to the related technical fields, are included in the scope of the present invention.
Claims (10)
1. An obstacle detouring method of an unmanned vehicle, comprising the steps of:
calculating an obstacle entry point and an obstacle exit point in a planned route based on the circle center and the detour radius of an obstacle, and drawing a first detour path between the obstacle entry point and the obstacle exit point;
acquiring a detouring point at different sides of the circle center of the barrier and a connecting line corresponding to a barrier entry point and a barrier exit point in a first detouring path, calculating a first included angle between the detouring point at different sides and the barrier entry point and the barrier exit point, and determining a second detouring path according to the size relationship between the first included angle and a preset angle;
determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour;
and splicing the starting point of the planned route, the pre-bypassing starting point, the second bypassing path, the pre-bypassing end point and the end point of the planned route.
2. The obstacle detouring method for the unmanned vehicle according to claim 1, wherein the calculating of the obstacle entry point and the obstacle exit point in the planned route based on the center of the obstacle and the detouring radius comprises:
calculating a bypassing radius according to the radius of the obstacle and a preset safe distance;
and sequentially traversing each path point in the planned route, and judging whether the distance between each path point and the center of the obstacle is equal to the bypassing radius, if so, taking the path point close to the starting point of the planned route as an obstacle entry point, and taking the path point close to the end point of the planned route as an obstacle exit point.
3. The obstacle detouring method of the unmanned vehicle as claimed in claim 1, wherein determining the second detouring path according to a magnitude relationship between the first included angle and a preset angle comprises:
calculating preset angles between the circle center of the obstacle and the obstacle entry point and between the circle center of the obstacle and the obstacle exit point;
and judging whether the first included angle is smaller than or equal to the preset angle, if so, adding the bypassing points to a second bypassing path, and if not, adding the bypassing points of different sides to the second bypassing path when all the first included angles are larger than the preset angle.
4. The obstacle detouring method for the unmanned vehicle according to claim 1, wherein the determining the pre-detouring start point and the pre-detouring end point according to a second angle between the planned route and the front and rear end points of the second detouring path in the paths before and after the detouring includes:
judging whether a second included angle between a path point of the planned route in the route before detour and a starting point of a second detour route is equal to a safety included angle or not, if so, taking the path point as a pre-detour starting point, and if not, taking the starting point of the second detour route as the pre-detour starting point when all the second included angles are not equal to preset angles;
judging whether a second included angle between a path point of the planned path in the bypassed path and a second bypassing path terminal point is equal to a safety included angle or not, if so, the path point is a pre-bypassing terminal point, and if not, when all the second included angles are not equal to preset angles, taking the second bypassing path terminal point as a pre-bypassing terminal point.
5. The obstacle detour method for an unmanned vehicle according to claim 1, wherein the splicing the starting point of the planned route, the pre-detour starting point, the second detour path, the pre-detour end point and the end point of the planned route comprises:
taking a path from a starting point to the pre-detour starting point in the planned route as a first planned path, and taking a path from a terminal point to the pre-detour terminal point in the planned route as a second planned path;
performing linear point supplement on the pre-detour starting point to the starting point of a second detour path to obtain a first pre-detour path, and performing linear point supplement on the end point of the second detour path to the pre-detour end point to obtain a second pre-detour path;
and splicing the first planning path, the first pre-bypassing path, the second pre-bypassing path and the second planning path in sequence.
6. An obstacle detour terminal for an unmanned vehicle, comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor when executing the computer program implements the steps of:
calculating an obstacle entry point and an obstacle exit point in a planned route based on the circle center and the detour radius of an obstacle, and drawing a first detour path between the obstacle entry point and the obstacle exit point;
acquiring a detouring point at different sides of the circle center of the barrier and a connecting line corresponding to a barrier entry point and a barrier exit point in a first detouring path, calculating a first included angle between the detouring point at different sides and the barrier entry point and the barrier exit point, and determining a second detouring path according to the size relationship between the first included angle and a preset angle;
determining a pre-detour starting point and a pre-detour end point according to a second included angle between the planned route and the front and rear end points of the second detour route in the routes before and after detour;
and splicing the starting point of the planned route, the pre-bypassing starting point, the second bypassing path, the pre-bypassing end point and the end point of the planned route.
7. The obstacle detour terminal of claim 6, wherein the calculating of the obstacle entry point and the obstacle exit point in the planned route based on the center of the obstacle and the detour radius comprises:
calculating a bypassing radius according to the radius of the obstacle and a preset safe distance;
and sequentially traversing each path point in the planned route, and judging whether the distance between each path point and the center of the obstacle is equal to the bypassing radius, if so, taking the path point close to the starting point of the planned route as an obstacle entry point, and taking the path point close to the end point of the planned route as an obstacle exit point.
8. The obstacle detour terminal of claim 6, wherein the determining of the second detour path according to the magnitude relation between the first included angle and the preset angle comprises:
calculating preset angles between the circle center of the obstacle and the obstacle entry point and between the circle center of the obstacle and the obstacle exit point;
and judging whether the first included angle is smaller than or equal to the preset angle, if so, adding the bypassing points to a second bypassing path, and if not, adding the bypassing points of different sides to the second bypassing path when all the first included angles are larger than the preset angle.
9. The obstacle detour terminal of claim 6, wherein the determining of the pre-detour start point and the pre-detour end point according to a second angle between the planned route and the front and rear end points of the second detour path in the paths before and after the detour comprises:
judging whether a second included angle between a path point of the planned path in the path before detour and a starting point of a second detour path is equal to a safety included angle or not, if so, the path point is a pre-detour starting point, and if not, when all the second included angles are not equal to preset angles, the second detour path starting point is taken as a pre-detour starting point;
judging whether a second included angle between a path point of the planned path in the bypassed path and a second bypassing path terminal point is equal to a safety included angle or not, if so, the path point is a pre-bypassing terminal point, and if not, when all the second included angles are not equal to preset angles, taking the second bypassing path terminal point as a pre-bypassing terminal point.
10. The obstacle detour terminal of an unmanned vehicle according to claim 6, wherein the splicing of the planned route start point, the pre-detour start point, the second detour path, the pre-detour end point and the planned route end point comprises:
taking a path from a starting point to the pre-detour starting point in the planned route as a first planned path, and taking a path from a terminal point to the pre-detour terminal point in the planned route as a second planned path;
performing linear point supplement on the pre-detour starting point to the starting point of a second detour path to obtain a first pre-detour path, and performing linear point supplement on the end point of the second detour path to the pre-detour end point to obtain a second pre-detour path;
and splicing the first planning path, the first pre-bypassing path, the second pre-bypassing path and the second planning path in sequence.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210398036.4A CN114995374A (en) | 2022-04-12 | 2022-04-12 | Obstacle bypassing method and terminal for unmanned vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210398036.4A CN114995374A (en) | 2022-04-12 | 2022-04-12 | Obstacle bypassing method and terminal for unmanned vehicle |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114995374A true CN114995374A (en) | 2022-09-02 |
Family
ID=83023487
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210398036.4A Pending CN114995374A (en) | 2022-04-12 | 2022-04-12 | Obstacle bypassing method and terminal for unmanned vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114995374A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115309168A (en) * | 2022-10-11 | 2022-11-08 | 天地科技股份有限公司 | Underground unmanned vehicle control method and device |
-
2022
- 2022-04-12 CN CN202210398036.4A patent/CN114995374A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115309168A (en) * | 2022-10-11 | 2022-11-08 | 天地科技股份有限公司 | Underground unmanned vehicle control method and device |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110260867B (en) | Method, equipment and device for determining and correcting neutral position in robot navigation | |
| US9834251B2 (en) | Method for steering a vehicle | |
| EP4102186A1 (en) | Method for constructing self-driving map and related device | |
| CN111301409A (en) | Parking path planning method and device, vehicle and storage medium | |
| CN109669446B (en) | Regression guide line searching method and device and automatic moving equipment | |
| CN113353103A (en) | Method, device, equipment and medium for controlling speed of curve vehicle | |
| JP6750365B2 (en) | Parking assistance method and parking assistance device | |
| Chen et al. | An enhanced dynamic Delaunay triangulation-based path planning algorithm for autonomous mobile robot navigation | |
| CN109827584B (en) | Path planning method and device, electronic equipment and storage medium | |
| CN114995374A (en) | Obstacle bypassing method and terminal for unmanned vehicle | |
| CN110749901A (en) | Autonomous mobile robot, map splicing method and device thereof, and readable storage medium | |
| CN112327826A (en) | Path planning method, device, equipment and medium | |
| JP2021191658A (en) | Driving assisting device, drive assisting method, and drive assisting program | |
| CN110069066B (en) | Unmanned vehicle tracking method and terminal | |
| US20230294684A1 (en) | Method of controlling autonomous vehicle, electronic device, and storage medium | |
| CN113296099B (en) | Method and terminal for automatically generating avoidance path according to obstacle | |
| CN112445221A (en) | Automatic working system and regression method | |
| CN115542896A (en) | Robot path generation method, system and storage medium | |
| CN114355932A (en) | Robot control method and device and robot | |
| CN114018261A (en) | Path width determination method, robot, and computer-readable storage medium | |
| CN111413982A (en) | Method and terminal for planning tracking routes of multiple vehicles | |
| CN118097999B (en) | Parking space identification method, device, equipment and medium | |
| Kiss et al. | A planning method to obtain good quality paths for autonomous cars | |
| CN114779771A (en) | Unmanned vehicle tracing planning route adjusting method and terminal | |
| CN117889866B (en) | Navigation path planning method, device and 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 |