A method and device for robot path selection
Through obstacle trajectory prediction and path sampling, combining collision cost and path length cost, the optimal path of the robot is selected, which solves the problem of difficult integration of obstacle states in the prior art and improves the safety and efficiency of robot operation.
Patent Information
- Application Number
- CN202310056875.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-01-17
- Publication Date
- 2025-07-22
- Estimated Expiration
- 2043-01-17
AI Technical Summary
The prior art is difficult to effectively combine the obstacle state to make optimal path selection in robot path planning, resulting in multiple emergency obstacle avoidances when the robot is traveling, increasing time and danger.
Through obstacle perception, the obstacle speed and angular velocity are calculated, and the obstacle velocity is assumed to be uniformly turned and motion, the obstacle trajectory is predicted, the collision possibility is judged based on path sampling, and the collision value is given, and the path length cost is selected.
It improves the safety and traffic efficiency of robot operation, simplifies the path selection process, and realizes efficient path planning.
Smart Images
Figure CN116027789B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of robot path planning, and particularly provides a robot path selection method and device. Background Art
[0002] With the progress of robot navigation technology, a variety of path planning methods have been developed. However, in the selection of the optimal path, it is usually difficult to judge in combination with the state of obstacles, resulting in the need for multiple emergency obstacle avoidance maneuvers during the movement of the robot, which greatly increases the time for the robot to reach the destination and also poses a certain danger. Summary of the Invention
[0003] The present invention aims at the above-mentioned deficiencies of the prior art and provides a robot path selection method with strong practicability.
[0004] A further technical task of the present invention is to provide a robot path selection device with reasonable design, safety and applicability.
[0005] The technical solution adopted by the present invention to solve its technical problems is as follows:
[0006] A robot path selection method. First, the robot plans multiple candidate paths according to an algorithm, then performs obstacle perception, calculates the speed and angular velocity of the obstacle through the change rate of the obstacle position, assumes that the obstacle moves at a uniform speed and uniform turn, and predicts the trajectory of the obstacle according to a certain step size;
[0007] Finally, make a path selection. For the multiple candidate feasible paths planned by the robot, sample them according to the same step size, combine the predicted obstacle trajectory, calculate whether the robot will collide with the obstacle when running along a certain candidate path in the future for a period of time, and assign different collision cost values cost1 to the multiple paths according to the probability of collision;
[0008] For the multiple paths planned, assign different cost values cost2 according to the different path lengths; add the collision cost cost1 and the path length cost cost2 of each path to obtain the total cost of each path. The path with the minimum total cost is the optimal path of the robot.
[0009] Further, an obstacle perception component is composed of a camera, a depth sensor and a lidar. The depth sensor perceives the positions of surrounding obstacles at a certain frequency;
[0010] Assume that the coordinates of the obstacle sensed by the sensor at the nearest moment are (x1, y1), the time is t1, the coordinates of the previous frame are (x2, y2), the time is t2, and the coordinates of the frame before the previous frame are (x3, y3), then the speed of the obstacle at this time can be calculated as Denoted as v, the angular velocity is Denoted as yawrate, the traveling angle is Denoted as yaw.
[0011] Furthermore, assume that the safety distance of the robot is L. Obstacles outside the distance L from the robot are considered to pose no risk of collision. Then, the obstacles are predicted according to the step size S, where
[0012] Assume that the obstacle moves at a constant speed and with a constant turn. The time when the obstacle reaches the next step point is The coordinates are (x1 + S * cos(yaw), y1 + S * sin(yaw)), and the traveling angle is
[0013] Furthermore, when making a path selection, first sample each planned path according to the step size S from the current position of the robot. The number of sampling points is N, and the coordinates of each sampling point and the time when the robot reaches that point are recorded;
[0014] After sampling, traverse the sampling points of the first planned trajectory to determine whether there will be a collision with each predicted trajectory point of the obstacle. The collision condition is that the distance between the robot path planning point and the obstacle trajectory prediction point is less than L.
[0015] Furthermore, for the points where a collision may occur, calculate the time difference between the robot reaching the collision point and the obstacle reaching the collision point, and then find the minimum time difference among all collision points. Take the absolute value and denote it as tc1. If there is no collision point, the collision cost of this path is recorded as 0. If tc1 is 0, directly reject this path and mark it as infeasible;
[0016] For other cases, record the collision cost cost1 of this planned path as And so on, calculate the minimum collision time difference between each planned path and the predicted trajectory of the obstacle, calculate the corresponding collision cost, and then normalize each collision cost.
[0017] Furthermore, for different planned paths, assign different cost values according to the ratio of the distance to the destination and perform normalization, denoted as cost2. The longer the distance, the greater the cost.
[0018] Furthermore, for the paths that are not marked as infeasible, calculate the total cost cost1 + K * cost2, where K is a positive number representing the weight of the path distance cost cost2, and is set according to the actual application scenario of the robot. The planned path with the minimum total cost is the finally selected optimal planned path.
[0019] A robot path selection device, comprising: at least one memory and at least one processor;
[0020] The at least one memory is used for storing machine-readable programs;
[0021] The at least one processor is used for calling the machine-readable program to execute a robot path selection method.
[0022] Compared with the prior art, a robot path selection method and device of the present invention have the following prominent beneficial effects:
[0023] By predicting the trajectory of an obstacle, combining the predicted trajectory of the obstacle, judging collisions for the planned path of the robot, and combining the distance cost of the path, the path with the minimum cost is selected therefrom, which can greatly improve the safety and passing efficiency of the robot running, and at the same time has the advantages of being simple, efficient and easy to implement. BRIEF DESCRIPTION OF THE DRAWINGS
[0024] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained according to these drawings without creative efforts.
[0025] Attached Figure 1 is a schematic flow chart of a robot path selection method. DETAILED DESCRIPTION OF THE EMBODIMENTS
[0026] In order to enable those skilled in the art to better understand the solutions of the present invention, the following will further elaborate on the present invention in combination with specific embodiments. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.
[0027] The following gives a best embodiment:
[0028] As Figure 1 shown, in a robot path selection method in this embodiment, first, the robot plans multiple candidate paths according to an algorithm, and then performs obstacle perception. By calculating the change rate of the obstacle position, the speed and angular velocity of the obstacle are calculated. Assuming that the obstacle moves at a uniform speed and uniform turn, the trajectory of the obstacle is predicted at a certain step size.
[0029] Finally, make a path selection. For multiple candidate feasible paths planned by the robot, sample them at the same step size. Combine the predicted obstacle trajectories, calculate whether the robot will collide with obstacles when running along a certain candidate path in the future for a period of time, and assign different collision cost values cost1 to multiple paths according to the likelihood of collision.
[0030] For multiple paths planned, assign different cost values cost2 according to different path lengths; add the collision cost cost1 and the path length cost2 of each path to obtain the total cost of each path. The path with the minimum total cost is the optimal path of the robot.
[0031] Among them, the obstacle perception component consists of a camera, a depth sensor, and a lidar. The depth sensor senses the positions of surrounding obstacles at a certain frequency.
[0032] Assume that the coordinates of the obstacle sensed by the sensor at the most recent moment are (x1, y1), the time is t1, the coordinates of the previous frame are (x2, y2), the time is t2, and the coordinates of the frame before the previous one are (x3, y3), the time is t3. Then the speed of the obstacle at this time can be calculated as Denoted as v, and the angular velocity is Denoted as yawrate, and the traveling angle is Denoted as yaw.
[0033] Assume that the safety distance of the robot is L. Obstacles outside the distance L from the robot are considered not to pose a danger of collision. Then predict the obstacles according to the step size S, where
[0034] Assume that the obstacle moves at a uniform speed and with a uniform turn. The moment when the obstacle reaches the next step point is The coordinates are (x1 + S * cos(yaw), y1 + S * sin(yaw)), and the traveling angle is
[0035] Similarly, according to the time, coordinates, and traveling angle of the obstacle reaching the next step point, the time, coordinates, and traveling angle of reaching the next next step point can be predicted, and so on. For each obstacle, N trajectory points are predicted. The size of N is set according to actual needs, usually greater than 10 and less than 100.
[0036] When making a path selection, first sample each planned path from the current position of the robot at the step size S. The number of sampling points is N, and record the coordinates of each sampling point and the time when the robot reaches the point.
[0037] After sampling, traverse the sampled points of the first planned trajectory to determine whether collisions will occur with the predicted trajectory points of each obstacle. The collision condition is that the distance between the robot path planning point and the obstacle trajectory prediction point is less than L.
[0038] For the points where collisions may occur, calculate the time difference between the robot reaching the collision point and the obstacle reaching the collision point, and then find the minimum time difference among all collision points. Take the absolute value and denote it as tc1. If there are no collision points, the collision cost of this path is denoted as 0. If tc1 is 0, directly reject this path and mark it as infeasible.
[0039] For other cases, record the collision cost cost1 of this planned path as And so on, calculate the minimum collision time difference between each planned path and the predicted trajectory of the obstacle, calculate the corresponding collision cost, and then normalize each collision cost, that is, the maximum collision cost is denoted as 1, and the others are increased or decreased in proportion.
[0040] Then, for different planned paths, according to the proportion of the distance to the destination, assign different cost values and normalize them, that is, the cost of the path with the longest distance is denoted as 1, and the others are increased or decreased in proportion, denoted as cost2. The longer the distance, the greater the cost.
[0041] For the paths that are not marked as infeasible, calculate the total cost cost1 + K * cost2, where K is a positive number representing the weight of the path distance cost cost2, and is set according to the actual application scenario of the robot. The planned path with the minimum total cost is the finally selected optimal planned path.
[0042] Based on the above method, a robot path selection device in this embodiment includes: at least one memory and at least one processor;
[0043] The at least one memory is used to store machine-readable programs;
[0044] The at least one processor is used to call the machine-readable program and execute a robot path selection method.
[0045] The above specific implementation manners are only specific cases of the present invention. The patent protection scope of the present invention includes but is not limited to the above specific implementation manners. Any appropriate changes or substitutions made by any person skilled in the art in the technical field that meet the claims of a robot path selection method and device of the present invention shall fall within the patent protection scope of the present invention.
[0046] Although embodiments of the present invention have been shown and described, it will be understood by those of ordinary skill in the art that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention, and the scope of the present invention is defined by the appended claims and their equivalents.
Claims
1. A robot path selection method, characterized in that, First, the robot plans multiple candidate paths according to an algorithm, and then performs obstacle perception. By calculating the rate of change of the obstacle's position, the speed and angular velocity of the obstacle are obtained. Assuming that the obstacle moves at a constant speed and with a constant angular turn, the trajectory of the obstacle is predicted at a certain step size. Finally, a path is selected. For the multiple candidate feasible paths planned by the robot, sampling is performed at the same step size. Combining with the predicted obstacle trajectory, it is calculated whether the robot will collide with the obstacle when running along a certain candidate path in the future period of time. According to the probability of collision, different collision cost values cost1 are assigned to the multiple paths. For the multiple planned paths, different cost values cost2 are assigned according to the different lengths of the paths. The collision cost cost1 and the path length cost cost2 of each path are added together to obtain the total cost of each path. The path with the minimum total cost is the optimal path of the robot. An obstacle perception component is composed of a camera, a depth sensor, and a lidar. The depth sensor senses the positions of surrounding obstacles at a certain frequency. Assume that the coordinates of the obstacle sensed by the sensor at the most recent moment are (x1, y1), the time is t1, the coordinates of the previous frame are (x2, y2), the time is t2, and the coordinates of the frame before the previous one are (x3, y3), the time is t3. Then the speed of the obstacle at this time can be calculated as , denoted as v, and the angular velocity is , denoted as yawrate, and the traveling angle is yawrate*(t1 - t2)+ , denoted as yaw; Assume that the safety distance of the robot is L, and obstacles outside the distance L from the robot are considered not to pose a risk of collision with you. Then, the obstacles are predicted according to the step size S, where ; Assume that the obstacle moves at a constant speed and with a constant turn, and the time when the obstacle reaches the next step point is , the coordinates are (x1 + S * cos(yaw), y1 + S * sin(yaw)), and the traveling angle is yawrate * ( ) + yaw; When making a path selection, first, each planned path is sampled from the current position of the robot at a step size S. The number of sampling points is N, and the coordinates of each sampling point and the time when the robot reaches the point are recorded. After sampling, the sampling points of the first planned trajectory are traversed to determine whether there will be a collision with each predicted obstacle trajectory point. The collision condition is that the distance between the robot path planning point and the obstacle trajectory prediction point is less than L. For the points where a collision may occur, calculate the time difference between the robot reaching the collision point and the obstacle reaching the collision point, and then find the minimum time difference among all collision points. Take the absolute value and record it as tc1. If there is no collision point, the collision cost of this path is recorded as 0. If tc1 is 0, this path is directly negated and marked as infeasible. For other cases, record the collision cost cost1 of this planned path as , and so on, calculate the minimum collision time difference between each planned path and the predicted trajectory of the obstacle, calculate the corresponding collision cost, and then normalize each collision cost.
2. The method for a robot path selection according to claim 1, wherein For different planned paths, different cost values are assigned according to the ratio of the distances to the destination and are normalized, denoted as cost2. The longer the distance, the greater the cost.
3. A robot path selection method according to claim 2, characterized in that, For the paths not marked as infeasible, calculate the total cost cost1 + K * cost2, where K is a positive number representing the weight of the path distance cost cost2 and is set according to the actual application scenario of the robot. The planned path with the minimum total cost is the finally selected optimal planned path.
4. A robot path selection device, characterized in that, Including: At least one memory and at least one processor; The at least one memory is used to store machine-readable programs; The at least one processor is used to call the machine-readable program and execute the method described in any one of claims 1 to 3.
Citation Information
Patent Citations
Dynamic window approach using optimal reciprocal collision avoidance cost-critic
CA3076498A1
Driving track planning method and system of unmanned vehicle at crossroad, and storage medium
CN112068545A