A Local Path Optimization Method Based on Geometric Model Constraints

Abstract

This invention discloses a local path optimization method based on geometric model constraints. The steps include: a) establishing a planar geometric model; b) determining the minimum rotation radius R of the autonomous vehicle; c) selecting the maximum turning angle th and an arithmetic sequence a; d) calculating the attitude difference sequence b after rotating by one angle based on the arithmetic sequence a in step c; e) calculating all possible attitudes of the autonomous vehicle after moving forward a certain distance from the path starting point, by traversing each attitude; f) checking whether the coordinates of each point in each attitude are within the obstacle; g) calculating dynamic programming (dp); h) selecting the attitude with the smallest dp as the starting attitude and repeating steps e and f; i) the found point represents the optimized path. This local path optimization method based on geometric model constraints enables smoother and more traceable autonomous vehicle paths, and allows the autonomous vehicle to avoid obstacles in a more reasonable way and with a lower risk of collision.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle path planning technology, and in particular to a local path optimization method based on geometric model constraints. Background Technology

[0002] The A* algorithm is one of the more common heuristic search algorithms, which is widely used in the field of path optimization. The unique feature of the A* algorithm is that it introduces global information when examining each possible node in the shortest path, estimates the distance of the current node from the destination, and uses it as a measure of the probability that the node is on the shortest path.

[0003] It should be noted that the path planned using the A* algorithm has the following drawbacks, specifically:

[0004] Defect 1: The path planned using the A* algorithm changes direction significantly during actual tracking, which can easily lead to tracking deviation from the path.

[0005] Defect 2: In some narrow places where turning is required, the path planned using the A* algorithm may not be able to pass through the autonomous vehicle or may easily scrape against the corner. Summary of the Invention

[0006] The purpose of this invention is to provide a local path optimization method based on geometric model constraints to address the shortcomings of existing technologies. This method enables autonomous vehicle paths to be smoother and more trackable, and allows autonomous vehicles to avoid obstacles in a more reasonable way and with less likelihood of collisions.

[0007] To achieve the above objectives, the present invention is implemented through the following technical solutions.

[0008] A local path optimization method based on geometric model constraints includes the following steps:

[0009] Step a: Establish the planar geometric model of the autonomous vehicle: Assign coordinates (x, y, z) to each point on the vehicle's body. n y n The points are listed and arranged as a sequence, which describes the set of all points of the autonomous vehicle in its own coordinate system, thus obtaining the planar geometric model of the autonomous vehicle.

[0010] Step b: Determine the minimum rotation radius R of the unmanned vehicle: Select an angle difference dt, then the x-axis difference of the unmanned vehicle after moving by an angle difference dt is dx = R × sin(dt), and the y-axis difference is dy = RR × cos(dt);

[0011] Step c: Select the maximum turning angle th of the unmanned vehicle when it travels 0.5×R distance with the minimum turning radius, and select an arithmetic sequence a=(-n×dt, ...-3×dt, -2×dt, -1×dt, 0, 1×dt, 2×dt, 3×dt...n×dt) from the range greater than -th and less than th, where n×dt is less than th;

[0012] Step d: Calculate the attitude difference sequence b = (R×sin(a), RR×cos(a), a) after rotating by one angle, based on the arithmetic sequence a in step c;

[0013] Step e: Starting from the path's starting point (px0, py0, th0), calculate all possible poses (px0, py0, th0) + b after the autonomous vehicle has moved forward a certain distance.

[0014] By traversing each posture, the coordinates of each point on the body of the unmanned vehicle under each posture are calculated as follows: xSucc = x+dx×cos(dt)-dy×sin(dt) , ySucc = y + dx × sin(dt) + dy × cos(dt);

[0015] Step f: Check on the map whether the coordinates of each point on the body of the autonomous vehicle in each attitude are within the obstacle; if they are within the obstacle, remove the corresponding attitude; if they are not within the obstacle, put the corresponding attitude into a set open.

[0016] Step g: Calculate the distance dp1 from the original A* algorithm planned path for each posture, and calculate the distance dp2 from the coordinates of the vehicle's front end in the corresponding posture to the original A* algorithm planned path. Let dp = dp1 + dp2.

[0017] Step h: Select the one with the smallest dp as the starting pose, and repeat steps e and f to calculate all poses for the next step and store them in the set open, until the last selected pose is less than the threshold distance to the end of the path, then the loop ends.

[0018] Step i: Find the starting pose sequentially from the last pose. The points found are the optimized paths.

[0019] In step a, the rotation center point of the unmanned vehicle is taken as the origin of the coordinate system, the positive direction of the vehicle's front is the positive direction of the X-axis, and the left side of the vehicle's front, perpendicular to the positive direction of the X-axis, is the positive direction of the Y-axis.

[0020] In step b, the angle difference dt is less than 2 degrees.

[0021] Compared with the prior art, the present invention has the following beneficial effects, specifically:

[0022] 1. The local path optimization method based on geometric model constraints of the present invention can make the path of the unmanned vehicle smoother and more traceable;

[0023] 2. The local path optimization method based on geometric model constraints of the present invention enables unmanned vehicles to avoid obstacles in a more reasonable way and are less likely to collide with obstacles. Detailed Implementation

[0024] The present invention will now be described in conjunction with specific embodiments.

[0025] A local path optimization method based on geometric model constraints includes the following steps:

[0026] Step a: Establish the planar geometric model of the autonomous vehicle: Assign coordinates (x, y, z) to each point on the vehicle's body. n y n The points are listed and arranged as a sequence, which describes the set of all points of the autonomous vehicle in its own coordinate system, thus obtaining the planar geometric model of the autonomous vehicle.

[0027] Step b: Determine the minimum rotation radius R of the unmanned vehicle: Select an angle difference dt, then the x-axis difference of the unmanned vehicle after moving by an angle difference dt is dx = R × sin(dt), and the y-axis difference is dy = RR × cos(dt);

[0028] Step c: Select the maximum turning angle th of the unmanned vehicle when it travels 0.5×R distance with the minimum turning radius, and select an arithmetic sequence a=(-n×dt, ...-3×dt, -2×dt, -1×dt, 0, 1×dt, 2×dt, 3×dt...n×dt) from the range greater than -th and less than th, where n×dt is less than th;

[0029] Step d: Calculate the attitude difference sequence b = (R×sin(a), RR×cos(a), a) after rotating by one angle, based on the arithmetic sequence a in step c;

[0030] Step e: Starting from the path's starting point (px0, py0, th0), calculate all possible poses (px0, py0, th0) + b after the autonomous vehicle has moved forward a certain distance.

[0031] By traversing each posture, the coordinates of each point on the body of the unmanned vehicle under each posture are calculated as follows: xSucc = x+dx×cos(dt)-dy×sin(dt) , ySucc = y + dx × sin(dt) + dy × cos(dt);

[0032] Step f: Check on the map whether the coordinates of each point on the body of the autonomous vehicle in each attitude are within the obstacle; if they are within the obstacle, remove the corresponding attitude; if they are not within the obstacle, put the corresponding attitude into a set open.

[0033] Step g: Calculate the distance dp1 from the original A* algorithm planned path for each posture, and calculate the distance dp2 from the coordinates of the vehicle's front end in the corresponding posture to the original A* algorithm planned path. Let dp = dp1 + dp2.

[0034] Step h: Select the one with the smallest dp as the starting pose, and repeat steps e and f to calculate all poses for the next step and store them in the set open, until the last selected pose is less than the threshold distance to the end of the path, then the loop ends.

[0035] Step i: Find the starting pose sequentially from the last pose. The points found are the optimized paths.

[0036] It should be noted that in step a, the rotation center point of the unmanned vehicle is taken as the origin of the coordinate system, the positive direction of the vehicle's front is the positive direction of the X-axis, and the left side of the vehicle's front, perpendicular to the positive direction of the X-axis, is the positive direction of the Y-axis.

[0037] Additionally, in step b, the angle difference dt is less than 2 degrees.

[0038] Through the above methods and steps, the local path optimization method based on geometric model constraints of the present invention can make the autonomous vehicle path smoother and more trackable, and make the autonomous vehicle path avoid obstacles more reasonably and less likely to collide with obstacles.

[0039] The above description is only a preferred embodiment of the present invention. For those skilled in the art, there will be changes in the specific implementation and application scope based on the ideas of the present invention. The content of this specification should not be construed as a limitation of the present invention.

Claims

1. A local path optimization method based on geometric model constraints, characterized in that, It includes the following steps, specifically: Step a: Establish the planar geometric model of the autonomous vehicle: Assign coordinates (x, y, z) to each point on the vehicle's body. n y n The points are listed and arranged as a sequence, which describes the set of all points of the autonomous vehicle in its own coordinate system, thus obtaining the planar geometric model of the autonomous vehicle. Step b: Determine the minimum rotation radius R of the unmanned vehicle: Select an angle difference dt, then the x-axis difference of the unmanned vehicle after moving by an angle difference dt is dx = R × sin(dt), and the y-axis difference is dy = RR × cos(dt); Step c: Select the maximum turning angle th of the unmanned vehicle when it travels 0.5×R distance with the minimum turning radius, and select an arithmetic sequence a=(-n×dt, ...-3×dt, -2×dt, -1×dt, 0, 1×dt, 2×dt, 3×dt...n×dt) from the range greater than -th and less than th, where n×dt is less than th; Step d: Calculate the attitude difference sequence b = (R×sin(a), RR×cos(a), a) after rotating by one angle based on the arithmetic sequence a in step c; Step e: Starting from the path's starting point (px0, py0, th0), calculate all possible poses (px0, py0, th0) + b after the autonomous vehicle has moved forward a certain distance. By traversing each posture, the coordinates of each point on the body of the unmanned vehicle under each posture are calculated as follows: xSucc = x+dx×cos(dt)-dy×sin(dt) , ySucc = y + dx × sin(dt) + dy × cos(dt); Step f: Check on the map whether the coordinates of each point on the body of the autonomous vehicle in each attitude are within the obstacle; if they are within the obstacle, remove the corresponding attitude; if they are not within the obstacle, put the corresponding attitude into a set open. Step g: Calculate the distance dp1 from the original A* algorithm planned path for each posture, and calculate the distance dp2 from the coordinates of the vehicle's front end in the corresponding posture to the original A* algorithm planned path. Let dp = dp1 + dp2. Step h: Select the one with the smallest dp as the starting pose, and repeat steps e and f to calculate all poses for the next step and store them in the set open, until the last selected pose is less than the threshold distance to the end of the path, then the loop ends. Step i: Find the starting pose sequentially from the last pose. The points found are the optimized paths.

2. The local path optimization method based on geometric model constraints according to claim 1, characterized in that: In step a, the rotation center point of the unmanned vehicle is taken as the origin of the coordinate system, the positive direction of the vehicle's front is the positive direction of the X-axis, and the left side of the vehicle's front, perpendicular to the positive direction of the X-axis, is the positive direction of the Y-axis.

3. The local path optimization method based on geometric model constraints according to claim 1, characterized in that: In step b, the angle difference dt is less than 2 degrees.