Obstacle bypassing method and terminal for unmanned vehicle

Abstract

The present invention discloses an obstacle bypassing method and terminal for an unmanned vehicle, which calculates an obstacle entry point and an obstacle exit point based on the center of the obstacle and the bypass radius, and calculates a first bypass path; obtains bypass points on different sides of the center of the obstacle relative to the connecting line of the obstacle entry point and the obstacle exit point in the first bypass path, and calculates the first angles between the bypass points on different sides and the obstacle entry point and the obstacle exit point, and determines a second bypass path according to the magnitude relationship between the first angle and a preset angle, which can ensure that the unmanned vehicle smoothly enters the bypass path. According to the second angle between the planned route in the path before and after the bypass and the front and rear end points of the second bypass path, the pre-detour starting point and the pre-detour end point are determined to avoid sharp turns when entering the bypass path, further ensuring that the unmanned vehicle can smoothly enter the bypass path.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicle control, and in particular to an obstacle circumvention method and a terminal for an unmanned vehicle. Background Art

[0002] At present, when the command and control platform sets a planned tracking route for the unmanned vehicle, the terrain is not necessarily flat and open, and it can go straight at will. Sometimes obstacles will be encountered on the tracking route. The operator marks certain obstacles on the command and control platform as a circular obstacle with a fixed radius and a certain point as the center. When planning the route, the route points need to bypass these circular obstacles to prevent the unmanned vehicle from encountering obstacles during driving. However, in the existing technology, the route bypassing the circular obstacle is not smooth enough, and sharp turns may occur, resulting in rollover. Summary of the invention

[0003] The technical problem to be solved by the present invention is to provide a method and terminal for unmanned vehicle to bypass obstacles, which can realize smooth bypassing of the unmanned vehicle.

[0004] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0005] A method for bypassing obstacles for an unmanned vehicle, comprising the steps of:

[0006] Calculating an obstacle entry point and an obstacle exit point in the planned route based on the center of the obstacle and the detour radius, and drawing a first detour path between the obstacle entry point and the obstacle exit point;

[0007] Obtaining detour points on different sides of the center of the obstacle relative to a connecting line of an obstacle entry point and an obstacle exit point in the first detour path, and calculating first angles between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determining a second detour path according to a magnitude relationship between the first angle and a preset angle;

[0008] Determine the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path;

[0009] 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 are spliced ​​together.

[0010] In order to solve the above technical problems, another technical solution adopted by the present invention is:

[0011] An obstacle bypassing terminal for an unmanned vehicle includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the following steps are implemented:

[0012] Calculating an obstacle entry point and an obstacle exit point in the planned route based on the center of the obstacle and the detour radius, and drawing a first detour path between the obstacle entry point and the obstacle exit point;

[0013] Obtaining detour points on different sides of the center of the obstacle relative to a connecting line of an obstacle entry point and an obstacle exit point in the first detour path, and calculating first angles between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determining a second detour path according to a magnitude relationship between the first angle and a preset angle;

[0014] Determine the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path;

[0015] 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 are spliced ​​together.

[0016] The beneficial effects of the present invention are: calculating the obstacle entry point and obstacle exit point based on the center of the obstacle and the detour radius, and calculating the first detour path; obtaining the detour points on different sides of the center of the obstacle relative to the connecting line of the obstacle entry point and obstacle exit point in the first detour path, and calculating the first angle between the detour points on different sides and the obstacle entry point and obstacle exit point, and determining the second detour path according to the relationship between the first angle and the preset angle, which can ensure that the unmanned vehicle smoothly enters the detour path. According to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path, the pre-detour starting point and pre-detour end point are determined to avoid sharp turns when entering the detour path, further ensuring that the unmanned vehicle can smoothly enter the detour path. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 A flowchart of an obstacle bypassing method for an unmanned vehicle according to an embodiment of the present invention;

[0018] Figure 2 This is a structural diagram of an obstacle bypassing terminal for an unmanned vehicle according to an embodiment of the present invention;

[0019] Figure 3 A schematic diagram of an unmanned vehicle detouring according to an embodiment of the present invention;

[0020] Description of labels:

[0021] 1. An obstacle bypassing terminal for an unmanned vehicle; 2. A memory; 3. A processor. DETAILED DESCRIPTION

[0022] In order to explain the technical content, achieved objectives and effects of the present invention in detail, the following is an explanation in combination with the implementation modes and the accompanying drawings.

[0023] Please refer to Figure 1 and Figure 3 , an embodiment of the present invention provides an obstacle bypassing method for an unmanned vehicle, comprising the steps of:

[0024] Calculating an obstacle entry point and an obstacle exit point in the planned route based on the center of the obstacle and the detour radius, and drawing a first detour path between the obstacle entry point and the obstacle exit point;

[0025] Obtaining detour points on different sides of the center of the obstacle relative to a connecting line of an obstacle entry point and an obstacle exit point in the first detour path, and calculating first angles between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determining a second detour path according to a magnitude relationship between the first angle and a preset angle;

[0026] Determine the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path;

[0027] 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 are spliced ​​together.

[0028] From the above description, it can be seen that the beneficial effects of the present invention are: calculating the obstacle entry point and obstacle exit point based on the center of the obstacle and the detour radius, and calculating the first detour path; obtaining the detour points on different sides of the center of the obstacle relative to the connecting line of the obstacle entry point and the obstacle exit point in the first detour path, and calculating the first angle between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determining the second detour path according to the relationship between the first angle and the preset angle, which can ensure that the unmanned vehicle smoothly enters the detour path. According to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path, the pre-detour starting point and pre-detour end point are determined to avoid sharp turns when entering the detour path, further ensuring that the unmanned vehicle can smoothly enter the detour path.

[0029] Further, 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 includes:

[0030] Calculate the detour radius according to the radius of the obstacle and the preset safety distance;

[0031] Traverse each path point in the planned route in turn, and determine whether the distance between each path point and the center of the obstacle is equal to the detour radius. If so, take the path point close to the starting point of the planned route as the obstacle entry point, and take the path point close to the end point of the planned route as the obstacle exit point.

[0032] From the above description, it can be seen that the obstacle entry point and obstacle exit point are determined according to the distance between each path point in the planned route and the center of the obstacle, which facilitates the subsequent calculation of the detour path and the pre-detour path.

[0033] Further, determining the second detour path according to the magnitude relationship between the first angle and the preset angle includes:

[0034] Calculating the preset angles between the center of the obstacle and the obstacle entry point and the obstacle exit point;

[0035] Determine whether the first angle is less than or equal to the preset angle. If so, add the detour point to the second detour path. If not, when all of the first angles are greater than the preset angle, add the detour points on different sides to the second detour path.

[0036] From the above description, it can be seen that by judging whether the first angle is less than or equal to the preset angle, the angle at which the unmanned vehicle enters the second detour path can be increased. When all the first angles are greater than the preset angle, it proves that the current path can be smoothly detoured, thereby flexibly obtaining the detour path.

[0037] Further, the determining of the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path comprises:

[0038] Determine whether the second angle between the path point in the path before the detour of the planned route and the starting point of the second detour path is equal to the safety angle, if so, the path point is the pre-detour starting point, if not, when all the second angles are not equal to the preset angle, the starting point of the second detour path is used as the pre-detour starting point;

[0039] Determine whether the second angle between the path point in the planned route after the detour and the end point of the second detour path is equal to the safety angle. If so, the path point is the pre-detour end point. If not, when all of the second angles are not equal to the preset angle, the end point of the second detour path is used as the pre-detour end point.

[0040] From the above description, it can be seen that by judging the second angle and the safety angle, the pre-detour starting point and the pre-detour end point can be obtained. The pre-detour starting point and the pre-detour end point can enable the unmanned vehicle to smoothly enter the second detour path without deviating significantly from the planned path.

[0041] Furthermore, the step of 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:

[0042] The path between the starting point of the planned route and the starting point of the pre-detour is used as the first planned path, and the path between the end point of the planned route and the end point of the pre-detour is used as the second planned path;

[0043] Filling in points on a straight line from the starting point of the pre-detour to the starting point of the second detour path to obtain a first pre-detour path, and filling in points on a straight line from the end point of the second detour path to the end point of the pre-detour to obtain a second pre-detour path;

[0044] The first planned path, the first pre-detour path, the second detour path, the second pre-detour path and the second planned path are sequentially spliced.

[0045] From the above description, it can be seen that by filling in straight lines from the starting point of the pre-detour to the starting point of the second detour path, and from the end point of the second detour path to the end point of the pre-detour, the pre-detour path can be obtained, which is convenient for path splicing to obtain a complete unmanned vehicle obstacle detour path.

[0046] Please refer to Figure 2 Another embodiment of the present invention provides an obstacle bypassing terminal for an unmanned vehicle, comprising a memory, a processor, 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:

[0047] Calculating an obstacle entry point and an obstacle exit point in the planned route based on the center of the obstacle and the detour radius, and drawing a first detour path between the obstacle entry point and the obstacle exit point;

[0048] Obtaining detour points on different sides of the center of the obstacle relative to a connecting line of an obstacle entry point and an obstacle exit point in the first detour path, and calculating first angles between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determining a second detour path according to a magnitude relationship between the first angle and a preset angle;

[0049] Determine the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path;

[0050] 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 are spliced ​​together.

[0051] From the above description, it can be seen that the beneficial effects of the present invention are: calculating the obstacle entry point and obstacle exit point based on the center of the obstacle and the detour radius, and calculating the first detour path; obtaining the detour points on different sides of the center of the obstacle relative to the connecting line of the obstacle entry point and the obstacle exit point in the first detour path, and calculating the first angle between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determining the second detour path according to the relationship between the first angle and the preset angle, which can ensure that the unmanned vehicle smoothly enters the detour path. According to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path, the pre-detour starting point and pre-detour end point are determined to avoid sharp turns when entering the detour path, further ensuring that the unmanned vehicle can smoothly enter the detour path.

[0052] Further, 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 includes:

[0053] Calculate the detour radius according to the radius of the obstacle and the preset safety distance;

[0054] Traverse each path point in the planned route in turn, and determine whether the distance between each path point and the center of the obstacle is equal to the detour radius. If so, take the path point close to the starting point of the planned route as the obstacle entry point, and take the path point close to the end point of the planned route as the obstacle exit point.

[0055] From the above description, it can be seen that the obstacle entry point and obstacle exit point are determined according to the distance between each path point in the planned route and the center of the obstacle, which facilitates the subsequent calculation of the detour path and the pre-detour path.

[0056] Further, determining the second detour path according to the magnitude relationship between the first angle and the preset angle includes:

[0057] Calculating the preset angles between the center of the obstacle and the obstacle entry point and the obstacle exit point;

[0058] Determine whether the first angle is less than or equal to the preset angle. If so, add the detour point to the second detour path. If not, when all of the first angles are greater than the preset angle, add the detour points on different sides to the second detour path.

[0059] From the above description, it can be seen that by judging whether the first angle is less than or equal to the preset angle, the angle at which the unmanned vehicle enters the second detour path can be increased. When all the first angles are greater than the preset angle, it proves that the current path can be smoothly detoured, thereby flexibly obtaining the detour path.

[0060] Further, the determining of the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path comprises:

[0061] Determine whether the second angle between the path point in the path before the detour of the planned route and the starting point of the second detour path is equal to the safety angle, if so, the path point is the pre-detour starting point, if not, when all the second angles are not equal to the preset angle, the starting point of the second detour path is used as the pre-detour starting point;

[0062] Determine whether the second angle between the path point in the planned route after the detour and the end point of the second detour path is equal to the safety angle. If so, the path point is the pre-detour end point. If not, when all of the second angles are not equal to the preset angle, the end point of the second detour path is used as the pre-detour end point.

[0063] From the above description, it can be seen that by judging the second angle and the safety angle, the pre-detour starting point and the pre-detour end point can be obtained. The pre-detour starting point and the pre-detour end point can enable the unmanned vehicle to smoothly enter the second detour path without deviating significantly from the planned path.

[0064] Furthermore, the step of 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:

[0065] The path between the starting point of the planned route and the starting point of the pre-detour is used as the first planned path, and the path between the end point of the planned route and the end point of the pre-detour is used as the second planned path;

[0066] Filling in points on a straight line from the starting point of the pre-detour to the starting point of the second detour path to obtain a first pre-detour path, and filling in points on a straight line from the end point of the second detour path to the end point of the pre-detour to obtain a second pre-detour path;

[0067] The first planned path, the first pre-detour path, the second detour path, the second pre-detour path and the second planned path are sequentially spliced.

[0068] From the above description, it can be seen that by filling in straight lines from the starting point of the pre-detour to the starting point of the second detour path, and from the end point of the second detour path to the end point of the pre-detour, the pre-detour path can be obtained, which is convenient for path splicing to obtain a complete unmanned vehicle obstacle detour path.

[0069] The above-mentioned obstacle bypassing method and terminal of an unmanned vehicle of the present invention are applicable to bypassing obstacles when the unmanned vehicle has a planned path. The following is an explanation through specific implementation methods:

[0070] Embodiment 1

[0071] Please refer to Figure 1 , a method for bypassing obstacles of an unmanned vehicle, comprising the steps of:

[0072] S1. Calculate an obstacle entry point and an obstacle exit point in a planned route based on the center of the obstacle and the detour radius, and draw a first detour path between the obstacle entry point and the obstacle exit point.

[0073] S11. Calculate a detour radius according to the radius of the obstacle and a preset safety distance.

[0074] Specifically, when circling an obstacle, the unmanned vehicle will only circumvent it when it cannot touch the obstacle. A safety distance rx needs to be set to ensure that the unmanned vehicle does not collide with the obstacle when circling. Then the actual circumvention radius of the unmanned vehicle should be rdis=r+rx, where r is the radius of the obstacle, which in some embodiments can be the length between the center of the obstacle and the farthest point of the edge of the obstacle.

[0075] S12. Traverse each path point in the planned route in turn, and determine whether the distance between each path point and the center of the obstacle is equal to the detour radius. If so, take the path point close to the starting point of the planned route as the obstacle entry point, and take the path point close to the end point of the planned route as the obstacle exit point.

[0076] Specifically, each path point on the planned route is traversed, and the distance is calculated based on the longitude and latitude of each path point and the longitude and latitude of the obstacle circle center. It is determined whether the distance between the path point and the circle center is equal to rdis. If so, the path point close to the starting point of the planned route is used as the obstacle entry point, and the path point close to the end point of the planned route is used as the obstacle exit point.

[0077] S2. Obtain detour points on different sides of the center of the obstacle relative to a connecting line between an obstacle entry point and an obstacle exit point in the first detour path, and calculate first angles between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determine a second detour path based on a size relationship between the first angle and a preset angle.

[0078] S21. Calculate preset angles between the center of the obstacle and the obstacle entry point and the obstacle exit point.

[0079] Specifically, using point I, the center of the circle center, and point O, the spatial coordinate vector function in the system is used to determine whether the center of the circle is on the left or right side of the IO line (isLeft), and obtain the angle angle of the three points.

[0080] S22. Determine whether the first angle is less than or equal to the preset angle. If so, add the detour point to the second detour path. If not, when all of the first angles are greater than the preset angle, add the detour points on different sides to the second detour path.

[0081] Specifically, the point tempP in the first detour path is traversed, and the detour curve point (tempP) and the point O are used to obtain whether the detour curve point is on the left or right side of the IO line (tempIsLeft).

[0082] Get the angle tempAngle of the three points and the distance dis from the detour curve point to point I. If tempIsLeft is opposite to isLeft, and tempAngle is less than or equal to angle, then add the point to the second detour path r2.

[0083] When all of the first angles are greater than a preset angle, the detour points on different sides are added to the second detour path r2.

[0084] In some embodiments, the waypoints in the second detour path are sorted according to dis.

[0085] S3. Determine the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path.

[0086] S31. Determine whether the second angle between a path point in the path of the planned route before the detour and the starting point of the second detour path is equal to the safety angle. If so, the path point is the pre-detour starting point. If not, when all of the second angles are not equal to the preset angle, the starting point of the second detour path is used as the pre-detour starting point.

[0087] Specifically, the index (Iindex) of point I in the entire planned route is obtained, starting with 0 and ending with Iindex, and a set of path points is intercepted from the entire planned path, that is, the set of planned path points before entering the detour.

[0088] In order to prevent the angle of the planned route entering the second detour path from being too narrow, causing the unmanned vehicle to overturn due to sharp turns during driving, it is necessary to judge the entry detour angle of the planned waypoint set before the detour.

[0089] Traverse the set of planned paths before detour from the end to the beginning:

[0090] The starting point of the second detour path is regarded as point r20, the points traversed in the planned path before the detour are regarded as point p, and the previous point of point p in the planned path is regarded as point grandP. Then, the included angle degree among points r20, p, and grandP can be obtained.

[0091] If the degree is less than a certain angle (such as 150°), skip this point and proceed to the next point until a point a that meets the conditions is found and the subscript i of the point is recorded.

[0092] If the degrees are all greater than a specific angle (such as 150°), the starting point of the second detour path is used as the pre-detour starting point a.

[0093] S32. Determine whether a second angle between a path point in the planned route after the detour and the end point of the second detour path is equal to a safety angle. If so, the path point is the pre-detour end point.

[0094] Specifically, the index (Oindex) of point O in the entire planned route is obtained, and a set of path points is extracted from the entire planned path, namely, the set of planned route points after detour, with Oindex as the start and the end point as the end.

[0095] In order to prevent the angle of the planned route after the second detour from being too narrow, causing the unmanned vehicle to roll over due to sharp turns during driving, it is necessary to judge the detour angle of r3.

[0096] Traverse the set of planned paths after detour:

[0097] The end point of the second detour path is regarded as point r2i, the points traversed in the planned path after the detour are regarded as point p, and the previous point of point p in the planned path is regarded as point grandP. Then, the included angle degree among points r2i, p, and grandP can be obtained.

[0098] If the degree is less than a certain angle (such as 150°), skip this point and proceed to the next point until a point c that meets the conditions is found and the subscript j of the point is recorded.

[0099] If the degrees are all greater than a specific angle (such as 150°), the end point of the second detour path is used as the pre-detour end point c.

[0100] S4. Connect 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.

[0101] S41: taking a path from a starting point in the planned route to the pre-detour starting point as a first planned path, and taking a path from an end point in the planned route to the pre-detour end point as a second planned path.

[0102] Specifically, starting with 0 and ending with i, a set of path points is extracted from the planned path before the detour and assigned to r1.

[0103] Taking j as the starting point and the end point as the end point, a set of path points is intercepted from the planned path after the detour and assigned to r3.

[0104] S42, fill in the points of a straight line from the starting point of the pre-detour to the starting point of the second detour path to obtain a first pre-detour path, and fill in the points of a straight line from the end point of the second detour path to the end point of the pre-detour to obtain a second pre-detour path.

[0105] Specifically, the last point of r1 is supplemented with the first point of r2 to obtain the path point set r12. The last point of r2 is supplemented with the first point of r3 to obtain the path point set r23.

[0106] S43: sequentially connect the first planned path, the first pre-detour path, the second detour path, the second pre-detour path, and the second planned path.

[0107] The five sets are concatenated in the order of r1, r12, r2, r23, and r3 to obtain the final modified backend planning route.

[0108] Embodiment 2

[0109] Please refer to Figure 3 This embodiment provides an application scenario where the planned path within the range of the obstacle is not a straight line, specifically:

[0110] The method for determining the second detour path is specifically as follows:

[0111] Step 1: Use the three points I, the center of the circle center, and the point O to determine whether the center of the circle is on the left or right side of the IO line (isLeft) through the spatial coordinate vector function in the system, and obtain the angle angle of the three points.

[0112] Step 2: Traverse the point tempP in the first detour path, and use point I, the detour curve point (tempP), and point O to determine whether the detour curve point is on the left or right side of the IO line (tempIsLeft).

[0113] Step 3: Get the angle tempAngle of the three points and the distance dis from the detour curve point to point I. If tempIsLeft is opposite to isLeft, and tempAngle is less than or equal to angle, then add the point to the second detour path r2.

[0114] The method for determining the pre-detour starting point and the pre-detour end point is as follows:

[0115] Get the index (Iindex) of point I in the entire planned route, starting with 0 and ending with Iindex, and extract a set of path points from the entire planned path, that is, the set of planned path points before entering the detour.

[0116] Traverse the set of planned paths before detour from the end to the beginning:

[0117] The starting point of the second detour path is regarded as point r20, the points traversed in the planned path before the detour are regarded as point p, and the previous point of point p in the planned path is regarded as point grandP. Then, the included angle degree among points r20, p, and grandP can be obtained.

[0118] If the degree is less than a certain angle (such as 150°), skip this point and proceed to the next point until a point a that meets the conditions is found, and record the subscript i of the point, and end the loop.

[0119] Determine whether the second angle between the path point in the planned route after the detour and the end point of the second detour path is equal to the safety angle. If so, the path point is the pre-detour end point.

[0120] Similarly, the above method is used to calculate the pre-detour endpoint.

[0121] Embodiment 3

[0122] Please refer to Figure 2 An obstacle bypassing 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. When the processor executes the computer program, each step of an obstacle bypassing method for an unmanned vehicle in embodiment one or two is implemented.

[0123] In summary, the obstacle bypassing method and terminal provided by the present invention for an unmanned vehicle calculates the obstacle entry point and obstacle exit point based on the center of the obstacle and the bypass radius, and calculates the first bypass path; obtains the bypass points on different sides of the center of the obstacle relative to the connecting line between the obstacle entry point and the obstacle exit point in the first bypass path, and calculates the first angle between the bypass points on different sides and the obstacle entry point and the obstacle exit point, and determines the second bypass path according to the relationship between the first angle and the preset angle, which can ensure that the unmanned vehicle smoothly enters the bypass path. According to the second angle between the planned route in the path before and after the bypass and the front and rear end points of the second bypass path, the pre-detour starting point and pre-detour end point are determined to avoid sharp turns when entering the bypass path, further ensuring that the unmanned vehicle can smoothly enter the bypass path.

[0124] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent transformations made using the contents of the present invention's specification and drawings, or directly or indirectly applied in related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for bypassing obstacles for an unmanned vehicle, characterized in that: Includes steps: Calculating an obstacle entry point and an obstacle exit point in the planned route based on the center of the obstacle and the detour radius, and drawing a first detour path between the obstacle entry point and the obstacle exit point; Obtaining detour points on different sides of a connecting line of an obstacle entry point and an obstacle exit point in a first detour path and a center of the obstacle, and calculating first angles between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determining a second detour path according to a magnitude relationship between the first angle and a preset angle, including: Calculating the preset angles between the center of the obstacle and the obstacle entry point and the obstacle exit point; Determine whether the first angle is less than or equal to the preset angle, if so, add the detour point to the second detour path, if not, when all the first angles are greater than the preset angle, add the detour points on different sides to the second detour path; Determine the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path; 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 are spliced ​​together.

2. The obstacle circumvention method for an unmanned vehicle according to claim 1, characterized in that: The calculation 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 includes: Calculate the detour radius according to the radius of the obstacle and the preset safety distance; Traverse each path point in the planned route in turn, and determine whether the distance between each path point and the center of the obstacle is equal to the detour radius. If so, take the path point close to the starting point of the planned route as the obstacle entry point, and take the path point close to the end point of the planned route as the obstacle exit point.

3. The obstacle circumvention method for an unmanned vehicle according to claim 1, characterized in that: Determining the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path comprises: Determine whether the second angle between the path point in the path before the detour of the planned route and the starting point of the second detour path is equal to the safety angle, if so, the path point is the pre-detour starting point, if not, when all the second angles are not equal to the preset angle, the starting point of the second detour path is used as the pre-detour starting point; Determine whether the second angle between the path point in the planned route after the detour and the end point of the second detour path is equal to the safety angle. If so, the path point is the pre-detour end point. If not, when all of the second angles are not equal to the preset angle, the end point of the second detour path is used as the pre-detour end point.

4. The obstacle circumvention method for an unmanned vehicle according to claim 1, characterized in that: The step of 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: The path between the starting point of the planned route and the starting point of the pre-detour is used as the first planned path, and the path between the end point of the planned route and the end point of the pre-detour is used as the second planned path; Filling in points on a straight line from the starting point of the pre-detour to the starting point of the second detour path to obtain a first pre-detour path, and filling in points on a straight line from the end point of the second detour path to the end point of the pre-detour to obtain a second pre-detour path; The first planned path, the first pre-detour path, the second detour path, the second pre-detour path and the second planned path are sequentially spliced.

5. An obstacle bypassing terminal for an unmanned vehicle, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the following steps are implemented: Calculating an obstacle entry point and an obstacle exit point in the planned route based on the center of the obstacle and the detour radius, and drawing a first detour path between the obstacle entry point and the obstacle exit point; Obtaining detour points on different sides of a connecting line of an obstacle entry point and an obstacle exit point in a first detour path and a center of the obstacle, and calculating first angles between the detour points on different sides and the obstacle entry point and the obstacle exit point, and determining a second detour path according to a magnitude relationship between the first angle and a preset angle, including: Calculating the preset angles between the center of the obstacle and the obstacle entry point and the obstacle exit point; Determine whether the first angle is less than or equal to the preset angle, if so, add the detour point to the second detour path, if not, when all the first angles are greater than the preset angle, add the detour points on different sides to the second detour path; Determine the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path; 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 are spliced ​​together.

6. The obstacle bypassing terminal for an unmanned vehicle according to claim 5, characterized in that: The calculation 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 includes: Calculate the detour radius according to the radius of the obstacle and the preset safety distance; Traverse each path point in the planned route in turn, and determine whether the distance between each path point and the center of the obstacle is equal to the detour radius. If so, take the path point close to the starting point of the planned route as the obstacle entry point, and take the path point close to the end point of the planned route as the obstacle exit point.

7. The obstacle bypassing terminal for an unmanned vehicle according to claim 5, characterized in that: Determining the pre-detour starting point and the pre-detour end point according to the second angle between the planned route in the path before and after the detour and the front and rear end points of the second detour path comprises: Determine whether the second angle between the path point in the path before the detour of the planned route and the starting point of the second detour path is equal to the safety angle, if so, the path point is the pre-detour starting point, if not, when all the second angles are not equal to the preset angle, the starting point of the second detour path is used as the pre-detour starting point; Determine whether the second angle between the path point in the planned route after the detour and the end point of the second detour path is equal to the safety angle. If so, the path point is the pre-detour end point. If not, when all of the second angles are not equal to the preset angle, the end point of the second detour path is used as the pre-detour end point.

8. The obstacle bypassing terminal for an unmanned vehicle according to claim 5, characterized in that: The step of 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: The path between the starting point of the planned route and the starting point of the pre-detour is used as the first planned path, and the path between the end point of the planned route and the end point of the pre-detour is used as the second planned path; Filling in points on a straight line from the starting point of the pre-detour to the starting point of the second detour path to obtain a first pre-detour path, and filling in points on a straight line from the end point of the second detour path to the end point of the pre-detour to obtain a second pre-detour path; The first planned path, the first pre-detour path, the second detour path, the second pre-detour path and the second planned path are sequentially spliced.

Images