Method for fast trajectory optimization of arbitrary shaped robots based on rc-esdf

Abstract

The application discloses a method for quickly optimizing a trajectory of a robot of an arbitrary shape based on RC-ESDF, which comprises the following steps: generating an occupancy grid map according to the shape of the robot, thereby establishing RC-ESDF on the mechanical system of the robot, wherein the ESDF value of a grid vertex inside the robot is negative and the absolute value is the nearest distance to the surface of the robot, and the ESDF value of a grid vertex outside the robot is 0; obtaining an initial trajectory of the robot; based on the initial trajectory, constructing a target function for trajectory optimization, wherein the target function comprises a collision penalty item for the joint optimization of the position and the yaw angle of the robot; and solving the target function to obtain an optimized trajectory, so that the robot moves according to the optimized trajectory. The application reduces the calculation consumption based on RC-ESDF and is suitable for robots of an arbitrary shape; and based on the joint optimization of the position and the yaw angle, the robot can pass through a narrow area in a complex environment in a suitable pose.

Description

Technical Field

[0001] This invention belongs to the field of robot autonomous navigation technology, and particularly relates to a fast trajectory optimization method for robots of arbitrary shapes based on RC-ESDF. Background Technology

[0002] Trajectory planning is a core module in robot autonomous navigation systems. In simple scenarios, the robot is often simplified to a point mass. However, to enable the robot to move flexibly in environments dense with obstacles, the robot's actual shape needs to be fully considered in trajectory planning; that is, a collision detection optimization term needs to be established between the robot's overall shape and the environment. This requires more complex calculations during trajectory optimization, and efficiently generating a collision-free trajectory with limited onboard computing resources remains a major challenge.

[0003] Existing trajectory planning methods considering robot shape, except for some task-specific methods [1, 2], are generally divided into two categories: corridor-based methods and ESDF (Euclidean Signed Distance Field)-based methods. Corridor-based methods use a series of continuous convex polyhedra to represent traversable regions in space and set analytical constraints to plan safe trajectories. However, corridors have strict requirements on the boundary between two adjacent convex hulls, which may lead to infeasible solutions when applied to narrow environments and non-convex robots due to the difficulty in meeting the requirements. ESDF-based methods establish a field that stores the distance to the nearest obstacle in the environment and construct constraints based on the distance gradient to keep the robot away from obstacles. This method is less conservative than methods that treat the robot as a point mass and inflate obstacles in the environment, but requires a trade-off between computational resources and the size of the field.

[0004] Considering the limitations of existing methods and the problems with planning methods that take into account robot shape, an ideal planning method should include the following points: (1) Efficiency: low computational resource consumption. (2) Accurate shape modeling: when applied to dense and complex scenes, the method should avoid trajectory optimization failures due to conservative modeling. (3) Universality: the method can be applied to robots of arbitrary shapes, even non-convex shapes.

[0005] Most mobile robot trajectory planning methods treat the robot as a point mass and expand the environmental obstacles according to the robot's radius to achieve safety constraints by ensuring that the point mass is within the passable area of ​​the environment. However, this conservative method does not take into account the actual shape of the robot, so it is difficult to use in complex environments. Existing technology [3] models the drone as a disk that flies and lands on a mobile platform, but this task-specific method is difficult to apply to other tasks. Existing technology [4] uses a search-based method to model the drone as an ellipse in order to make the drone pass through narrow gaps, and ensures safety constraints by checking the collision between the ellipse and the environment in each motion primitive. However, in environments with dense obstacles, this method uses a finer search resolution to improve the success rate of the search, which will cause a surge in computational overhead. In summary, this method of modeling the robot as a sphere is not accurate and not universal.

[0006] Some trajectory planning methods that consider the shape of the robot use corridor-based approaches. Generally, a corridor is represented by a series of polygonal or spherical convex hulls. Existing technology [5] proposes a baseline framework for autonomous racing of UAVs, where the actual shape of the UAV needs to be considered when the UAV is navigating in a narrow environment. This method models the robot as a convex polygon and constrains this convex polygon within a flight corridor to ensure trajectory safety. Existing technologies [6] and [7] use a series of rectangles as safety corridors. Although this method speeds up the establishment of corridors, it sacrifices most of the solution space. More importantly, the above corridor-based methods strictly require that the intersection of two adjacent convex hulls contains at least the overall shape of the robot; otherwise, there will be no solution. In addition, the generation of corridors is limited by the quality of the reference trajectory. If the reference trajectory cannot satisfy the dynamic constraints or the constraints of the overall shape of the robot, it is difficult to optimize the trajectory to a feasible solution. Furthermore, corridor-based methods are too conservative for non-convex robots.

[0007] Environment-based ESDF is also widely used in trajectory planning, storing the distance between each point in space and the nearest obstacle in the form of a map. An intuitive approach is to model the robot as a series of circles and ensure trajectory safety by querying whether the distance between the center of each circle in the environment ESDF and the nearest obstacle exceeds the radius of the circle. However, this conservative approach is difficult to use in narrow spaces. Although ESDF-based methods provide more environmental information than corridor-based methods, their computational overhead is not negligible. In trajectory optimization, we need to establish a large field to ensure that the trajectory is within the ESDF, so balancing the size of the field and the consumption of computational resources remains a challenging problem. Although existing techniques [8, 9] propose incremental methods for building ESDF, the generated ESDF always contains redundant information that is not helpful for planning. In summary, the excessive computational overhead of environment ESDF and the difficulty in determining the size of the field make it difficult to apply to trajectory planning that considers the robot's shape.

[0008] References:

[0009] [1] J.Ji, Z.Wang, Y.Wang, C.Xu, and F.Gao, "Mapless-planner: A robust and fast planning framework for aggressive autonomous flight without map fusion," in 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021, pp.6315-6321.

[0010] [2] S.Liu, K.Mohta, N.Atanasov, and V.Kumar, "Search-based motion planning for aggressive flight in se(3)," IEEE Robotics and Automation Letters, vol.3, no.3, pp.2439-2446, 2018.

[0011] [3] J. Ji, T. Yang, C. Xu, and F. Gao, "Real-time trajectory planning for aerial perching," arXiv preprint arXiv:2203.01061, 2022.

[0012] [4]S.Liu,K.Mohta,N.Atanasov,and V.Kumar,"Search-based motion planningfor aggressive flight in se(3),"IEEE Robotics and Automation Letters,vol.3,no.3,pp.2439-2446,2018.

[0013] [5]Z.Han,Z.Wang,N.Pan,Y.Lin,C.Xu,and F.Gao,"Fast-racing:An open-source strong baseline for se(3)planning in autonomous drone racing,"IEEERobotics and Automation Letters,vol.6,no.4,pp.8631-8638,2021.

[0014] [6]W.Ding,L.Zhang,J.Chen,and S.Shen,"Safe trajectory generation forcomplex urban environments using spatio-temporal semantic corridor,"IEEERobotics and Automation Letters,vol.4,no.3,pp.2997-3004,2019.

[0015] [7]S.Manzinger,C.Pek,and M.Althoff,"Using reachable sets fortrajectory planning of automated vehicles,"IEEE Transactions on IntelligentVehicles,vol.6,no.2,pp.232-248,2020.

[0016] [8] H.Oleynikova, Z.Taylor, M.Fehr, R.Siegwart, and J.Nieto, "Voxblox: Incremental 3deuclidean signed distance fields for onboard mav planning," in 2017 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), 2017, pp.1366-1373.

[0017] [9] L.Han, F.Gao, B.Zhou, and S.Shen, "Fiesta: Fast incremental euclideandistance fields for online motion planning of aerial robots," in 2019 IEEE / RSJInternational Conference on Intelligent Robots and Systems (IROS), 2019, pp.4423-4430. Summary of the Invention

[0018] To address the problems existing in the prior art, the purpose of this application is to provide a fast trajectory optimization method for robots of arbitrary shapes based on RC-ESDF.

[0019] According to a first aspect of the embodiments of this application, a fast trajectory optimization method for an arbitrary-shaped robot based on RC-ESDF is provided, comprising:

[0020] An occupied grid map is generated based on the robot's shape, thereby establishing an RC-ESDF in the robot's system. The ESDF value of the grid vertices inside the robot is negative and the absolute value is the distance to the nearest point on the robot's surface, while the ESDF value of the grid vertices outside the robot is 0.

[0021] Obtain the robot's initial trajectory;

[0022] Based on the initial trajectory, an objective function for trajectory optimization is constructed. The objective function is to minimize the smoothness of the robot position, the feasibility penalty of the robot position, the smoothness of the robot yaw angle, the feasibility penalty of the robot yaw angle, and the sum of the jointly optimized robot position and yaw angle collision penalty terms.

[0023] Solve the objective function to obtain an optimized trajectory, so that the robot can move according to the optimized trajectory.

[0024] Furthermore, an occupied grid map is generated based on the robot's shape, including:

[0025] In the robot system, the robot is processed into a grid with the same resolution. The area containing the robot body is considered to be occupied, and its corresponding grid vertex is recorded as 1. The unoccupied area is recorded as 0, thereby generating an occupied grid map based on the robot shape.

[0026] Furthermore, the initial trajectory is obtained using the A* algorithm or the RRT* algorithm.

[0027] Furthermore, the initial trajectory is parameterized as a uniform B-spline curve, which is derived from its degree p. b The time interval between nodes Δt, and N c control points {Q k ,Ψ k A uniquely determined piecewise polynomial, where Q k and Ψ k These are the control points for the position trajectory and the yaw angle trajectory, respectively.

[0028] Furthermore, when optimizing the trajectory, optimize N. c +1-2p b Control points and

[0029] Furthermore, the objective function is:

[0030]

[0031] Where Q and Ψ represent the control points of the position trajectory and yaw angle trajectory, respectively, and J ps and J pf It is the smoothness and feasibility penalty of the position, J ψs and J ψf It is the smoothness and feasibility penalty of the yaw angle, J c It is a joint optimization of the robot's position and yaw angle collision penalty term, λ ps ,λ pf ,λ ψs ,λ ψf ,λ c It is the weight of each penalty item.

[0032] Furthermore, the jointly optimized robot position and yaw angle collision penalty term J c for:

[0033]

[0034]

[0035] Among them, F c (p k ,ψ k ) is a potentially differentiable cost function. The trajectory will be related to the pose. The sum of the RC-ESDF values ​​of the points where the robot collides, N c This represents the number of control points on the trajectory.

[0036] According to a second aspect of the embodiments of this application, a rapid trajectory optimization device for an arbitrary-shape robot based on RC-ESDF is provided, comprising:

[0037] A module is established to generate an occupied grid map based on the robot's shape, thereby establishing RC-ESDF in the robot's mechanical system. The ESDF value of the grid vertices inside the robot is negative and the absolute value is the distance to the nearest robot surface, while the ESDF value of the grid vertices outside the robot is 0.

[0038] The acquisition module is used to acquire the robot's initial trajectory;

[0039] The construction module is used to construct an objective function for trajectory optimization based on the initial trajectory. The objective function is to minimize the smoothness of the robot position, the feasibility penalty of the robot position, the smoothness of the robot yaw angle, the feasibility penalty of the robot yaw angle, and the sum of the jointly optimized robot position and yaw angle collision penalty terms.

[0040] The solution module is used to solve the objective function to obtain an optimized trajectory, so that the robot can move according to the optimized trajectory.

[0041] According to a third aspect of the embodiments of this application, an electronic device is provided, comprising:

[0042] One or more processors;

[0043] Memory, used to store one or more programs;

[0044] When the one or more programs are executed by the one or more processors, the one or more processors perform the method as described in the first aspect.

[0045] According to a fourth aspect of the embodiments of this application, a computer-readable storage medium is provided, having stored thereon computer instructions, characterized in that, when executed by a processor, the instructions implement the steps of the method as described in the first aspect.

[0046] The technical solutions provided by the embodiments of this application may include the following beneficial effects:

[0047] As can be seen from the above embodiments, this application pre-establishes an RC-ESDF based on the robot's shape and mechanical system, using RC-ESDF to avoid collisions without requiring real-time updates, thus significantly reducing computational consumption. Based on RC-ESDF, this application employs joint optimization of position and yaw angle, simultaneously optimizing position and yaw angle in complex environments to ensure the robot passes through narrow areas with appropriate poses. Compared to environmental ESDF, RC-ESDF does not require real-time updates based on environmental information obtained from sensors, which greatly reduces computational resource consumption. Since environmental ESDF can describe obstacles of arbitrary shapes, RC-ESDF has natural applicability to robots of arbitrary shapes, even non-convex robots, meeting the requirements of ideal planning methods that consider robot shape. It can efficiently generate trajectories that conform to robot dynamics / kinematic constraints while considering the safety constraints of robot shape.

[0048] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and do not limit this application. Attached Figure Description

[0049] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0050] Figure 1 This is a flowchart illustrating a rapid trajectory optimization method for an arbitrary-shaped robot based on RC-ESDF, according to an exemplary embodiment.

[0051] Figure 2 This is a schematic diagram of an L-shaped robot corresponding to an RC-ESDF according to an exemplary embodiment, wherein (a) is an L-shaped robot and (b) is an RC-ESDF established based on the L-shaped robot.

[0052] Figure 3 This is a schematic diagram illustrating the process of a robot colliding with an obstacle according to an exemplary embodiment.

[0053] Figure 4 This is a schematic diagram comparing the trajectory of the present method with that of a method considering the overall shape constraints of the robot, according to an exemplary embodiment. (a) is a schematic diagram of the trajectory of the present method, (b) is a schematic diagram of the trajectory of method 1, (c) is a schematic diagram comparing the trajectory of method 2, (d) is a schematic diagram comparing the trajectory of method 3, (e) is a partial enlarged view of (b), and (f) is a partial enlarged view of (c).

[0054] Figure 5 This is a schematic diagram illustrating the yaw rate change of the present method and method 1 according to an exemplary embodiment.

[0055] Figure 6 This is a comparison chart illustrating the time required to establish an ESDF using the present method and method 3, according to an exemplary embodiment.

[0056] Figure 7 This is a block diagram illustrating a rapid trajectory optimization device for an arbitrary-shaped robot based on RC-ESDF, according to an exemplary embodiment.

[0057] Figure 8 This is a schematic diagram of an electronic device according to an exemplary embodiment. Detailed Implementation

[0058] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application.

[0059] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. The singular forms “a,” “the,” and “the” used in this application and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any or all possible combinations of one or more of the associated listed items.

[0060] It should be understood that although the terms first, second, third, etc., may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from one another. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "when," "when," or "in response to determination."

[0061] Figure 1 This is a flowchart illustrating a fast trajectory optimization method for an arbitrary-shaped robot based on RC-ESDF, according to an exemplary embodiment. Figure 1 As shown, this method, implemented in a terminal, may include the following steps:

[0062] Step S11: Generate an occupied grid map based on the robot's shape, thereby establishing an RC-ESDF in the robot's system, where the ESDF value of the grid vertices inside the robot is negative and the absolute value is the nearest distance to the robot's surface, and the ESDF value of the grid vertices outside the robot is 0.

[0063] Step S12: Obtain the robot's initial trajectory;

[0064] Step S13: Based on the initial trajectory, construct an objective function for trajectory optimization. The objective function is to minimize the smoothness of the robot position, the feasibility penalty of the robot position, the smoothness of the robot yaw angle, the feasibility penalty of the robot yaw angle, and the sum of the jointly optimized robot position and yaw angle collision penalty terms.

[0065] Step S14: Solve the objective function to obtain the optimized trajectory, so that the robot moves according to the optimized trajectory.

[0066] As can be seen from the above embodiments, this application pre-establishes an RC-ESDF based on the robot's shape and mechanical system, using RC-ESDF to avoid collisions without requiring real-time updates, thus significantly reducing computational consumption. Based on RC-ESDF, this application employs joint optimization of position and yaw angle, simultaneously optimizing position and yaw angle in complex environments to ensure the robot passes through narrow areas with appropriate poses. Compared to environmental ESDF, RC-ESDF does not require real-time updates based on environmental information obtained from sensors, which greatly reduces computational resource consumption. Since environmental ESDF can describe obstacles of arbitrary shapes, RC-ESDF has natural applicability to robots of arbitrary shapes, even non-convex robots, meeting the requirements of ideal planning methods that consider robot shape. It can efficiently generate trajectories that conform to robot dynamics / kinematic constraints while considering the safety constraints of robot shape.

[0067] In the specific implementation of step S11, an occupation grid map is generated according to the robot shape, thereby establishing RC-ESDF in the robot's mechanical system. The ESDF value of the grid vertex inside the robot is negative and its absolute value is the nearest distance to the robot surface, while the ESDF value of the grid vertex outside the robot is 0.

[0068] Specifically, in the robot system, the robot is processed into a grid with the same resolution. The area with the robot body is considered to be occupied, and its corresponding grid vertex is recorded as 1. The unoccupied area is recorded as 0, and an occupation grid map based on the robot shape is generated.

[0069] RC-ESDF, based on an occupied grid map generated from the robot's shape, employs an efficient algorithm with a complexity of O(n) (see PFFelzenszwalb and DPHuttenlocher, "Distance transforms of sampled functions," Theory of computing, vol. 8, no. 1, pp. 415-428, 2012). This algorithm calculates the distance from a point on the map to the nearest occupied grid cell; this distance is called the ESDF value. This algorithm requires RC-ESDF to be stored in grid form; this invention stores the ESDF values ​​of each grid vertex. Figure 2 As shown in (a) and (b), RC-ESDF (Robo-centric ESDF) is pre-established within the robot's mechanics, focusing only on the robot's shape rather than the morphology of obstacles in the environment. In practice, for a robot with a known shape, the ESDF value defined inside the robot is negative, its absolute value being the nearest distance to the robot's surface; the ESDF value defined outside the robot is 0. This avoids optimization discontinuities caused by abrupt boundary changes. Conversely, environmental ESDF can only address this issue by establishing a large field. RC-ESDF-based trajectory optimization ignores obstacle information outside the RC-ESDF, i.e., information about obstacles that do not collide with the robot, and only considers obstacle points that collide with the robot. The ESDF value and gradient information of any obstacle point in the RC-ESDF can be obtained through trilinear interpolation.

[0070] like Figure 3 As shown, it illustrates the generation of the gradient of the collision penalty term during a robot's collision with obstacles in its environment along an initial rough trajectory, within both world-system and machine-system frameworks. i For obstacle points that collide with the robot, the solid gray arrows represent the robot's movement direction along the initial trajectory. In the robot's system, obstacle points enter the RC-ESDF along the direction of the dashed gray arrows. The arrows on the obstacle points indicate the gradient direction calculated in the RC-ESDF. By summing the gradients of obstacle points entering the RC-ESDF at that moment, and based on the rotation relationship between the robot's system and the world frame, the gradient information is transformed onto the initial trajectory in the world frame. The gradient direction is shown by the arrows on the initial reference trajectory. The gradient information is then passed to the optimization of the robot's position and attitude according to the chain rule. The collision penalty cost for each trajectory point is obtained by summing the ESDF values ​​of each obstacle point entering the RC-ESDF. Since the ESDF does not have directional attributes, no coordinate system transformation is required.

[0071] In the specific implementation of step S12, the initial trajectory of the robot is obtained;

[0072] Specifically, the robot's starting position and target position are obtained, and an initial trajectory is generated through a path generation algorithm. Any existing path generation algorithm can be used, and in one embodiment, A* or RRT* is used.

[0073] In the specific implementation of step S13, based on the initial trajectory, an objective function for trajectory optimization is constructed. The objective function is to minimize the smoothness of the robot position, the feasibility penalty of the robot position, the smoothness of the robot yaw angle, the feasibility penalty of the robot yaw angle, and the sum of the jointly optimized robot position and yaw angle collision penalty terms.

[0074] Specifically, the robot's trajectory is represented as {x(t), y(t), ψ(t)}∈SE(2), where x(t) and y(t) represent the robot's x and y coordinates at time t, and ψ(t) represents the robot's yaw angle at time t. The trajectory is parameterized as a uniform B-spline curve, which is formed by its degree p. b The time interval between nodes Δt, and N c control points {Q k ,Ψ k A uniquely determined piecewise polynomial, where Q k and Ψ k These are the control points for the position trajectory and the yaw angle trajectory, respectively. In one embodiment, p can be used. b =3, then the total time for the entire trajectory is (N c -3)Δt. The velocity, acceleration, and jerk curves of the control point can be obtained from the following formula:

[0075]

[0076] Yaw angular velocity, acceleration, and jerk control point V Ψ,k A Ψ,k J Ψ,k The expression is:

[0077]

[0078] This application optimizes N. c +1-2p b Control points and p before and after b The control points do not participate in the optimization because their initial and final states of the trajectory are determined.

[0079] This application constructs a joint trajectory optimizer that simultaneously optimizes the robot's position and heading angle. It can generate a collision-free trajectory that considers the robot's shape and satisfies smoothness and feasibility constraints. For a ground robot with integrity constraints, the optimization problem can be written as follows:

[0080]

[0081] Among them, J ps and J pf It is the smoothness and feasibility penalty of the position, J ψs and J ψf It is the smoothness and feasibility penalty of the yaw angle, J c It is a joint optimization of the position and yaw angle collision penalty terms. λ ps ,λ pf ,λ ψs ,λ ψf ,λ c It is the weight of each penalty item.

[0082] Among them, J ps J pf J ψs and J ψf The calculation can be performed using existing algorithms, and this application does not limit it.

[0083] The collision penalty term is to ensure that the robot as a whole is at every constraint point. Keep away from obstacles to ensure a collision-free trajectory. Based on the properties of third-order B-splines, the constraint points p of the position trajectory and yaw angle trajectory are... k and ψ k They are defined as follows:

[0084]

[0085]

[0086] Where, p k =[x k ,y k ] T ψ is the center of rotation of the robot in the world coordinate system. k It's the yaw angle.

[0087] In this application, the collision penalty J c for:

[0088]

[0089]

[0090] Among them, F c (pk ,ψ k ) is a potentially differentiable cost function. The trajectory will be related to the pose. The sum of the RC-ESDF values ​​of the points where the robot collides:

[0091]

[0092] Among them, M d It is the number of collision points, d i It is the RC-ESDF value of the i-th collision point.

[0093] The world coordinates of the j-th vertex in the RC-ESDF are:

[0094]

[0095] Among them, M v It is the number of vertices, v b,j =[x b,j ,y b,j ] T This is the coordinate of the j-th vertex in the robot's architecture. Once the robot's shape is determined, v... b,j That remains unchanged. R k The rotation matrix represents the robot's rotation. We use axis-aligned bounding boxes (AABBs) to depict a rectangular region where the robot resides. The obstacle point q falls within the AABB box. w,i It can be converted to q under the machine system. b,i :

[0096]

[0097] This application uses H i,k To indicate in At the i-th collision point q b,i The RC-ESDF value. RC-ESDF value H i,k For p k and ψ k The gradient is:

[0098]

[0099]

[0100] Cost function F c (p k ,ψ k For p respectively k and ψ k The gradients are respectively:

[0101]

[0102]

[0103] In the specific implementation of step S14, the objective function is solved to obtain the optimized trajectory;

[0104] Specifically, the optimization problem in this application has two characteristics: first, the objective function adapts to the detected obstacles; second, quadratic terms dominate the objective function, making the function approximate as a quadratic expression. These two characteristics require the optimizer to restart quickly and utilize Hessian information to accelerate convergence. However, obtaining an accurate inverse Hessian matrix is ​​not easy, as it consumes a lot of computational resources. To avoid this problem, a quasi-Newton method is used to approximate the inverse Hessian matrix based on gradient information. The L-BFGS (Limited-memory Broyden–Fletcher–Goldfarb–Shanno) algorithm (《Liu DC, Nocedal J. On the limited memory BFGS method for large scale optimization[J].Mathematical programming,1989,45(1):503-528.》) algorithm is used as the solver to optimize the trajectory. L-BFGS is a quasi-Newton method that uses an approximate Hessian matrix to calculate the direction of the next iteration in the optimization process. The “Limited-memory” in its name refers to the fact that L-BFGS stores only a few previous iterations in memory, which reduces the amount of data involved in the optimization and improves the efficiency of large-scale optimization.

[0105] The method provided in this application (this method) is compared with environment-based ESDF methods, such as Fast-Planner and the two-circle model, and corridor-based methods, such as Fast-Racing.

[0106] like Figure 4 As shown, Figure 4 (a) in the diagram is a schematic diagram of the trajectory using this method. Figure 4(b) in the figure is a schematic diagram of the trajectory of Method 1 ("Li B, Zhang Y, Zhang Y, et al. Cooperative lane change motion planning of connected and automated vehicles: A stepwise computational framework[C] / / 2018 IEEE Intelligent Vehicles Symposium(IV).2018:334-338.") (two-circle model). Figure 4 (c) in the diagram is a trajectory comparison diagram of method 2 (《Han Z, Wang Z, Pan N, et al. Fast-Racing: An Open-Source StrongBaseline for SE(3)Planning in Autonomous Drone Racing[J].IEEE Robotics and Automation Letters,2021,6(4):8631-8638.》) (corridor-based method). Figure 4 (d) is a schematic diagram of trajectory comparison for method 3 (《Zhou B, Gao F, Wang L, et al. Robust and efficient quadrotortrajectory generation for fast autonomous flight[J].IEEE Robotics and Automation Letters,2019,4(4):3529-3536.》) (the whole-body version of Fast-Planner).

[0107] Figure 4 The two-circle model shown in (b) abstracts the robot's shape as a combination of two circles. This approach is too conservative in narrow areas, such as... Figure 4 As shown in (e), the robot gets blocked by obstacles due to its mismatched shape approximation. This causes a drastic change in the yaw angle, making it impossible to meet the feasibility constraints. The yaw rate changes of this method and Method 1 are as follows: Figure 5 As shown:

[0108] Figure 4 (c) The corridor-based method in the text represents the passable area in space as a corridor composed of a series of convex polygons. The results show that trajectory optimization fails because the intersection of the two convex hulls cannot encompass the overall shape of the machine, such as... Figure 4 As shown in (f) in the figure.

[0109] Figure 4 (d) is based on a whole-body version of Fast-Planner, which samples the robot surface and sums the ESDF values ​​of these points to construct safety constraints. Fast-Planner requires maintaining a 16×12m environmental ESDF, while the time required to generate the ESDF using this method is negligible (e.g., ...). Figure 6 (As shown).

[0110] As can be seen from the comparative experiments, the proposed method accurately describes the robot's shape, thus enabling the robot to be robust when traversing narrow areas. Furthermore, the RC-ESDF established under the robot system does not require real-time updates, effectively reducing the computational cost of trajectory optimization.

[0111] Corresponding to the aforementioned embodiments of the RC-ESDF-based rapid trajectory optimization method for arbitrary-shaped robots, this application also provides embodiments of an RC-ESDF-based rapid trajectory optimization device for arbitrary-shaped robots.

[0112] Figure 7 This is a block diagram illustrating a rapid trajectory optimization device for an arbitrary-shaped robot based on RC-ESDF, according to an exemplary embodiment. (Refer to...) Figure 7 The device may include:

[0113] Module 21 is established to generate an occupied grid map based on the robot's shape, thereby establishing an RC-ESDF in the robot's system. The ESDF value of the grid vertices inside the robot is negative and the absolute value is the distance to the nearest point on the robot's surface, while the ESDF value of the grid vertices outside the robot is 0.

[0114] Acquisition module 22 is used to acquire the robot's initial trajectory;

[0115] The construction module 23 is used to construct an objective function for trajectory optimization based on the initial trajectory. The objective function is to minimize the smoothness of the robot position, the feasibility penalty of the robot position, the smoothness of the robot yaw angle, the feasibility penalty of the robot yaw angle, and the sum of the jointly optimized robot position and yaw angle collision penalty terms.

[0116] The solver module 24 is used to solve the objective function to obtain an optimized trajectory, so that the robot can move according to the optimized trajectory.

[0117] Regarding the apparatus in the above embodiments, the specific manner in which each module performs its operation has been described in detail in the embodiments related to the method, and will not be elaborated upon here.

[0118] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this application according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0119] Accordingly, this application also provides an electronic device, including: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the above-described method for fast trajectory optimization of arbitrary-shaped robots based on RC-ESDF. Figure 8 The diagram shown is a hardware structure diagram of any device with data processing capabilities, used in an embodiment of the present invention to provide a fast trajectory optimization method for an arbitrary-shaped robot based on RC-ESDF. (Except for...) Figure 8 In addition to the processor, memory, and network interface shown, any data processing device in the embodiment may also include other hardware depending on the actual function of the data processing device, which will not be described in detail here.

[0120] Accordingly, this application also provides a computer-readable storage medium storing computer instructions, which, when executed by a processor, implement the aforementioned method for rapid trajectory optimization of an arbitrary-shape robot based on RC-ESDF. The computer-readable storage medium can be an internal storage unit of any data-processing device as described in any of the foregoing embodiments, such as a hard disk or memory. The computer-readable storage medium can also be an external storage device, such as a plug-in hard disk, smart media card (SMC), SD card, flash card, etc., equipped on the device. Furthermore, the computer-readable storage medium can include both internal storage units of any data-processing device and external storage devices. The computer-readable storage medium is used to store the computer program and other programs and data required by the data-processing device, and can also be used to temporarily store data that has been output or will be output.

[0121] Other embodiments of this application will readily occur to those skilled in the art upon consideration of the specification and practice of the disclosure herein. This application is intended to cover any variations, uses, or adaptations of this application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein.

[0122] It should be understood that this application is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope.

Claims

1. A fast trajectory optimization method for robots of arbitrary shapes based on RC-ESDF, characterized in that, include: An occupied grid map is generated based on the robot's shape, thereby establishing an RC-ESDF in the robot's system. The ESDF value of the grid vertices inside the robot is negative and the absolute value is the distance to the nearest point on the robot's surface, while the ESDF value of the grid vertices outside the robot is 0. Obtain the robot's initial trajectory; Based on the initial trajectory, an objective function for trajectory optimization is constructed. The objective function is to minimize the smoothness of the robot position, the feasibility penalty of the robot position, the smoothness of the robot yaw angle, the feasibility penalty of the robot yaw angle, and the sum of the jointly optimized robot position and yaw angle collision penalty terms. Solve the objective function to obtain an optimized trajectory, so that the robot can move according to the optimized trajectory; Among them, the jointly optimized robot position and yaw angle collision penalty term for: ， in, It is a potentially differentiable cost function. The trajectory will be related to the pose. The sum of the RC-ESDF values ​​at the points where the robot collides. This represents the number of control points on the trajectory.

2. The method according to claim 1, characterized in that, Generate an occupied grid map based on the robot's shape, including: In the robot system, the robot is processed into a grid with the same resolution. The area containing the robot body is considered to be occupied, and its corresponding grid vertex is recorded as 1. The unoccupied area is recorded as 0, thereby generating an occupied grid map based on the robot shape.

3. The method according to claim 1, characterized in that, The initial trajectory is obtained using the A* algorithm or the RRT* algorithm.

4. The method according to claim 1, characterized in that, The initial trajectory is parameterized as a uniform B-spline curve, which is determined by its degree. Inter-node time interval ,and Control points The uniquely determined piecewise polynomial, where and These are the control points for the position trajectory and the yaw angle trajectory, respectively.

5. The method according to claim 4, characterized in that, When performing trajectory optimization, optimize Control points and .

6. The method according to claim 1, characterized in that, The objective function is: ， in These represent the control points for the position trajectory and the yaw angle trajectory, respectively. and It is a penalty for the smoothness and feasibility of the location. and It is a penalty for the smoothness and feasibility of the yaw angle. It is a joint optimization of robot position and yaw angle collision penalty term. , It is the weight of each penalty item.

7. A rapid trajectory optimization device for arbitrary-shaped robots based on RC-ESDF, characterized in that, include: A module is established to generate an occupied grid map based on the robot's shape, thereby establishing RC-ESDF in the robot's mechanical system. The ESDF value of the grid vertices inside the robot is negative and the absolute value is the distance to the nearest robot surface, while the ESDF value of the grid vertices outside the robot is 0. The acquisition module is used to acquire the robot's initial trajectory; The construction module is used to construct an objective function for trajectory optimization based on the initial trajectory. The objective function is to minimize the smoothness of the robot position, the feasibility penalty of the robot position, the smoothness of the robot yaw angle, the feasibility penalty of the robot yaw angle, and the sum of the jointly optimized robot position and yaw angle collision penalty terms. The solution module is used to solve the objective function to obtain an optimized trajectory, so that the robot can move according to the optimized trajectory; Among them, the jointly optimized robot position and yaw angle collision penalty term for: ， in, It is a potentially differentiable cost function. The trajectory will be related to the pose. The sum of the RC-ESDF values ​​at the points where the robot collides. This represents the number of control points on the trajectory.

8. An electronic device, characterized in that, include: One or more processors; Memory, used to store one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any one of claims 1-6.

9. A computer-readable storage medium storing computer instructions thereon, characterized in that, When executed by the processor, this instruction implements the steps of the method as described in any one of claims 1-6.

Images