CN-116429136-B - RC-ESDF-based rapid trajectory optimization method for robot in any shape

CN116429136BCN 116429136 BCN116429136 BCN 116429136BCN-116429136-B

Abstract

The application discloses a robot rapid track optimization method based on RC-ESDF, which comprises the steps of generating an occupied grid map according to the shape of a robot, establishing RC-ESDF in a robot system of the robot, wherein ESDF values of grid vertexes inside the robot are negative and absolute values are distances to the surface of the robot nearest, ESDF values of grid vertexes outside the robot are 0, acquiring an initial track of the robot, constructing a track optimization objective function based on the initial track, wherein the objective function comprises joint optimization robot position and yaw angle collision penalty items, and solving the objective function to obtain an optimization track so that the robot moves according to the optimization track. The robot based on RC-ESDF reduces the calculation consumption, is suitable for robots of any shape, and can ensure that the robots pass through narrow areas in the environment in proper pose in complex environments based on joint optimization of positions and yaw angles.

Inventors

JIN RUI
GAO FEI
GENG SHUANG
WANG QIANHAO
XU CHAO

Assignees

浙江大学

Dates

Publication Date: 20260512
Application Date: 20230316

Claims (9)

1. The rapid trajectory optimization method of the robot in any shape based on RC-ESDF is characterized by comprising the following steps of: Generating an occupancy grid map from the robot shape, thereby establishing RC-ESDF in the body system of the robot, wherein the ESDF value of the grid vertices inside the robot is negative and the absolute value is the closest distance to the robot surface, and the ESDF value of the grid vertices outside the robot is 0; acquiring an initial track of a robot; Constructing a track optimized objective function based on the initial track, wherein the objective function is the sum of the smoothness of the minimum robot position, the feasibility penalty of the robot position, the smoothness of the yaw angle of the robot, the feasibility penalty of the yaw angle of the robot and the joint optimized robot position and yaw angle collision penalty; solving the objective function to obtain an optimized track so that the robot moves according to the optimized track; Wherein the joint optimized robot position and yaw angle collision penalty term The method comprises the following steps: , Wherein, the Is a potentially microscopic cost function that is a function of the cost, Is that the track will be in the same pose as the position The sum of the RC-ESDF values of the points where the robot involved the collision, Is the number of control points on the track.
2. The method of claim 1, wherein generating an occupancy grid map from the robot shape comprises: In the robot system, a robot or other resolution rasterization process is performed, wherein if an area of the robot body is considered to be occupied, the corresponding raster vertex is recorded as 1, and the unoccupied area is recorded as 0, so that an occupied raster map based on the robot shape is generated.
3. The method of claim 1, wherein the initial trajectory is obtained by an a-algorithm or an RRT-algorithm.
4. The method of claim 1, wherein the initial trajectory is parameterized as a uniform B-spline curve, the uniform B-spline curve being derived from its number of secondary values Time interval between nodes And (d) sum Control points Uniquely determined piecewise polynomials, wherein And Control points for the position track and the yaw angle track, respectively.
5. The method of claim 4, wherein the trajectory optimization is performed by optimizing Control points And 。
6. The method of claim 1, wherein the objective function is: , Wherein the method comprises the steps of Control points representing the position trajectory and the yaw angle trajectory respectively, And Is a smoothness and feasibility penalty for the location, And Is a smoothness and feasibility penalty for yaw angle, Is a joint optimized robot position and yaw angle collision penalty term, , Is the weight of each penalty term.
7. An arbitrary shape robot rapid trajectory optimization device based on RC-ESDF, which is characterized by comprising: a building module for generating an occupancy grid map from the robot shape, thereby building RC-ESDF in the body system of the robot, wherein the ESDF value of the grid vertex inside the robot is negative and the absolute value is the nearest distance to the robot surface, and the ESDF value of the grid vertex outside the robot is 0; The acquisition module is used for acquiring the initial track of the robot; The construction module is used for constructing a track optimized objective function based on the initial track, wherein the objective function is the sum of the smoothness of the minimum robot position, the feasibility penalty of the robot position, the smoothness of the yaw angle of the robot, the feasibility penalty of the yaw angle of the robot and the joint optimized robot position and yaw angle collision penalty; the solving module is used for solving the objective function to obtain an optimized track so that the robot moves according to the optimized track; Wherein the joint optimized robot position and yaw angle collision penalty term The method comprises the following steps: , Wherein, the Is a potentially microscopic cost function that is a function of the cost, Is that the track will be in the same pose as the position The sum of the RC-ESDF values of the points where the robot involved the collision, Is the number of control points on the track.
8. An electronic device, comprising: one or more processors; A memory for storing one or more programs; The one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of any of claims 1-6.
9. A computer readable storage medium having stored thereon computer instructions which, when executed by a processor, implement the steps of the method according to any of claims 1-6.

Description

RC-ESDF-based rapid trajectory optimization method for robot in any shape Technical Field The invention belongs to the technical field of autonomous navigation of robots, and particularly relates to a rapid trajectory optimization method of a robot in any shape based on RC-ESDF. Background Trajectory planning is a core module in a robotic autonomous navigation system. For some simple scenes, the robot is usually simplified into one particle, however, in order to make the robot flexibly move in an environment with dense obstacles, the real shape of the robot needs to be fully considered in track planning, namely, a collision detection optimization term of the shape of the whole machine of the robot and the environment is established. This requires more complex calculations in performing trajectory optimization, and how to efficiently generate a collision-free trajectory with limited on-board computing resources is a current challenge. Existing trajectory planning methods that take the shape of the robot into account are often divided into two categories, corridor-based methods and ESDF (Euclidean SIGNED DISTANCE FIELD, euclidean distance field) based methods, except for some task-specific methods [1,2 ]. Corridor-based methods use a series of consecutive convex polyhedrons to represent passable areas in space and set resolved constraints to plan a safe trajectory. However, hallways have stringent requirements for the intersection of two adjacent convex hulls, which when applied to narrow environments and non-convex shaped robots may result in no viable solution because of the difficulty in meeting the requirements. ESDF-based methods are to create a field that stores the distance to the nearest obstacle in the environment, construct constraints based on the gradient of the distance, and keep the robot away from the obstacle. This approach is less conservative than the approach where robots are considered particles and expand obstacles in the environment, but requires a tradeoff between computational resources and field size. In view of the limitations of the existing methods and the problems associated with planning methods that take into account the shape of the robot, the ideal planning method should include (1) high efficiency-low computational resources. (2) Accurate shape modeling the method should avoid trajectory optimization failure due to conservative modeling when applied to dense and complex scenarios. (3) General purpose this method can be applied to robots of arbitrary shape, even non-convex shape. Most mobile robot trajectory planning methods treat the robot as a particle and expand environmental obstacles according to the radius of the robot, and achieve security constraints by ensuring that the particle is within a passable area of the environment. However, this conservative approach does not take into account the actual shape of the robot and is therefore difficult to use in complex environments. The prior art [3] models the unmanned aerial vehicle as a disc and flies on a mobile platform, but the method aiming at the specific task is difficult to be widely applied to other tasks. The prior art [4] uses a search-based method, in order to make the unmanned aerial vehicle pass through the narrow slit, the unmanned aerial vehicle is modeled as an ellipse, and the security constraint is ensured by checking the collision of the ellipse with the environment in each motion element. However, in a dense obstacle environment, this approach uses finer search resolution to increase the success rate of the search, which can lead to a sudden increase in computational overhead. In summary, this approach to modeling robots as spheres is not accurate and universal. Some trajectory planning that takes into account the shape of the robot uses corridor-based methods. The general corridor is represented by a series of polygonal or spherical convex hulls. The prior art [5] provides a reference frame for autonomous racing of an unmanned aerial vehicle, and the actual shape of the unmanned aerial vehicle needs to be considered when the unmanned aerial vehicle shuttles in a narrow environment. The method models the robot as a convex polygon, and constrains the convex polygon in a flight corridor to ensure the safety of the track. The prior art [6] and [7] take a series of rectangles as safety corridors, although this approach speeds up the building speed of the corridors, he sacrifices most of the solution space. More importantly, the corridor-based approach described above strictly requires that the boundary between two adjacent convex hulls contain at least the overall shape of a robot, otherwise there will be no solution. In addition, the generation of the corridor is limited by the quality of the reference track, and if the reference track cannot meet the dynamic constraint or the constraint of the shape of the whole machine, the track is difficult to optimize a feasible solution. In addition, corridor-based met