Tractor trailer robot track generation method based on polynomial and symbolic distance field

Abstract

The invention discloses a tractor trailer robot track generation method based on a polynomial and a symbolic distance field, and the method comprises the steps: dividing grids through a point cloud map, estimating the curvature and normal direction of each grid, extracting obstacle information, and calculating a symbolic distance value corresponding to each grid; taking the initial pose of the tractor trailer robot as a root node, and based on the obstacle information, utilizing a multi-end-point heuristic graph search algorithm for spatial search in SE (2) to generate an initial path for arriving at the target area; and in combination with the symbol distance value corresponding to each grid, modeling the trajectory generation of the tractor trailer robot as a constraint optimization problem, and solving the constraint optimization problem by taking the initial path as an initial value to obtain a final trajectory. The problem that track generation is difficult due to complex kinematics, high state space dimension and deformable structure of the traction trailer robot is solved, the problems that an existing algorithm is low in efficiency and loses solution space are solved, and efficient track generation of the traction trailer robot is achieved.

Description

Technical Field

[0001] The present invention belongs to the technical field of trajectory generation for towing trailer robots, and particularly relates to a method for generating trajectories of towing trailer robots based on polynomials and signed distance fields. Background Art

[0002] A towing trailer robot is a type of robot composed of a drivable tractor and many non-powered trailers. The trailers are usually connected to the tractor or other trailers by hinges, so that the movement of the trailer can be controlled by controlling the movement of the tractor.

[0003] Robot trajectory generation technology (motion planning) refers to the ability of a robot to automatically plan a safe motion trajectory from the current point to the target point based on its own sensors' perception of the environment. It is an important part of the autonomous navigation framework, and towing trailer robots are no exception. Similar to car-like robots, the trajectory generation of towing trailer robots aims to plan a trajectory that can satisfy kinematic constraints and minimize time consumption as much as possible, so as to achieve smooth and safe navigation to the target area.

[0004] Although motion planning for car-like robots is a relatively mature field, these technologies cannot be directly applied to towing trailer robots because the states of both the tractor and the trailers need to be considered during motion. Specifically, compared with car-like robots, the difficulties in motion planning for towing trailer robots are mainly caused by the following three factors: 1) Complex kinematics: The hinge structure and non-drivability of the trailers bring more complex kinematic constraints. The coupling of the underactuated constraints of the trailers and the nonholonomic constraints of the tractor requires the planner to be able to handle more equality constraints. 2) High-dimensional state space: Each trailer on the robot adds three additional spatial dimensions to the robot, namely position and yaw angle, which greatly increases the time consumed by search- or sampling-based path planning algorithms. Similarly, for optimization-based methods, the increase in the state dimension also leads to an increase in the dimension of the optimization variables, which may result in more memory consumption and slower convergence speed. 3) Deformable structure: The hinge structure of the towing trailer robot results in its deformability, which increases the complexity of collision avoidance. Since the shape of the robot changes over time, collision avoidance constraints must be imposed on each unit of the robot. If there are multiple trailers, this challenge is even more obvious because additional inter-vehicle collision avoidance constraints must also be considered.

[0005] The motion planning of a tractor-trailer robot mainly falls into two categories: geometric-based methods and optimization-based methods. Geometric-based methods only plan the geometric path of the robot and design a path tracking controller to track it. These methods always search in a discrete space, and the obtained optimal solution is positively correlated with the search duration and spatial granularity. When considering higher-dimensional information in the robot state to find a more optimized solution, these methods need to explore a larger space, thus magnifying the problem of combinatorial explosion and seriously affecting the efficiency.

[0006] To find better solutions in such a large space, optimization-based methods have emerged. These methods usually model motion planning as an optimization problem and use numerical solvers to solve it. The initial values usually come from the geometric paths obtained by search or sampling methods. Trajectory optimization usually explores the optimal solution in a continuous space, and it can consider the information of higher-order states on the premise of ensuring local optimality. However, most current trajectory optimization algorithms model the optimization problem as a Nonlinear Model Predictive Control (NMPC) problem, which is not suitable for the generation of long trajectories because the complexity of the NMPC problem will increase sharply with the expansion of the planning horizon. At the same time, existing methods often need to construct a safety region from the environment using convex approximation through collision-free seed points before each optimization, resulting in the loss of the solution space. Summary of the Invention

[0007] Aiming at the problems of complex kinematics, high-dimensional state space, and difficult trajectory generation due to the deformable structure of the tractor-trailer robot, as well as the low efficiency and loss of solution space of existing algorithms, the purpose of the embodiments of this application is to provide a trajectory generation method for a tractor-trailer robot based on polynomials and signed distance fields, construct a signed distance field, use polynomials as the robot trajectory representation, model and solve the constrained optimization problem, and finally achieve efficient trajectory generation of the tractor-trailer robot.

[0008] According to the first aspect of the embodiments of this application, a trajectory generation method for a tractor-trailer robot based on polynomials and signed distance fields is provided, including:

[0009] Using a point cloud map, dividing grids and estimating the curvature and normal direction of each grid, extracting obstacle information and calculating the signed distance value corresponding to each grid;

[0010] Taking the initial pose of the tractor-trailer robot as the root node, based on the obstacle information, using a multi-endpoint heuristic graph search algorithm that searches in the SE(2) space to generate an initial path to reach the target area;

[0011] Combined with the signed distance value corresponding to each grid, the trajectory generation of the tractor-trailer robot is modeled as a constrained optimization problem. Using the initial path as the initial value, the constrained optimization problem is solved to obtain the final trajectory.

[0012] Furthermore, using the point cloud map, estimate the curvature and normal direction of each grid, extract obstacle information, and calculate the signed distance value corresponding to each grid, including:

[0013] For each grid p i , with p i as the center, find the points in the point cloud within the spherical region that just contains the size of the tractor n i is the number of found points, calculate the center coordinates of these points Covariance matrix Let the eigenvalues of the covariance matrix be λ i1 ≤ λ i2 ≤ λ i3 , then the normal direction i corresponding to grid p is the eigenvector corresponding to λ i1 , and the curvature value

[0014] If the third dimension value of is less than the threshold c norma l or is greater than the threshold κ pmax , then set p i as an obstacle grid, otherwise set p i as a traversable grid;

[0015] If p i is an obstacle grid, then set its signed distance value to the negative of the distance between it and the nearest traversable grid; if pi is a traversable grid, then set its signed distance value to the distance between it and the nearest obstacle grid.

[0016] Furthermore, with the initial pose of the tractor-trailer robot as the root node, based on the obstacle information, use a multi-endpoint heuristic graph search algorithm that searches in the SE(2) space to generate an initial path to reach the target area, including:

[0017] (Ⅰ) Based on the obstacle information, construct a grid map of the SE(2) space, and calculate the grid p start to which the initial pose (x0, y0, θ0) of the tractor-trailer robot belongs according to the occupancy range of the grid in the world coordinate system. Create a set cSet, create a search node nod s , set its total expected cost f s = 0, the current cost g s=0, set its parent node to empty and put it into the set oSet;

[0018] (Ⅱ) When oSet is not empty: take out f from oSet s The smallest node nod c And add cSet, if the corresponding grid belongs to one of the grids where the terminal position is not reached, then from nod c The parent nodes of the node are traced back one by one to obtain the path; if the grid to which the node belongs does not belong to any grid where the terminal position that has not been reached is located, but the straight-line distance from at least one terminal position that has not been reached is less than the threshold L oshot , then the minimum turning radius is R cmax The Dubins curve connects the node posture with all corresponding end postures. If the connection is successful and there is no occupied grid on the Dubins curve, the path is obtained;

[0019] (III) If no path is obtained in (II), then according to nod c The corresponding posture obtains the adjacent nodes under different speeds and steering angles and stores them in the set nei; for each node nei in nei i If the grid does not exist or is an obstacle grid, it will not be visited; on the contrary, if the grid exists and is a passable grid, the expansion cost tg = g is calculated snow +w d d i +w δ |δ i |+w dδ |dδ i |, where g snow for nod c The price, d i for nod c with nei i The distance, δ i is the input steering angle, dδ i is the steering angle difference from the previous node, w d ,w δ ,w dδ is a constant coefficient, assuming nei i The total expected cost f s f snei , the current cost g s g snei , the inspiration cost heu is the straight-line distance between it and the end point position, if nei i In oSet and its cost is g snei <tg, then update nei i g s =tg,f s =tg+heu, parent node is nodc ; If nei i is in cSet and g snei < tg, then update the g i of nei s = tg, f s = tg + heu, with the parent node being nod c , and remove it from cSet and readd it to oSet; If nei i does not belong to oSet and does not belong to cSet, then set the g i of nei s = tg, f s = tg + heu, with the parent node being nod c , and include it in oSet; Go to step (II);

[0020] (IV) If the paths corresponding to all end poses are found, then use the kinematic model of the towing trailer robot, at regular time steps, obtain the full state paths of all paths through numerical integration, calculate the path costs of all paths, and take the path with the minimum path cost as the initial path.

[0021] Furthermore, represent the target area using a convex polygon with M edges, and the end pose (x f , y f , θ fl ) is composed of the centroid position of the convex polygon and the direction angles θ fl corresponding to the normal directions of each edge in the world coordinate system, l = 1, 2,.., M.

[0022] Furthermore, represent the trajectory of the towing trailer robot using a piecewise quintic polynomial function traj(t) = [x(s), y(s), s(t), θ1(t), θ2(t),...,, θ N (t)] T , x(s), y(s) represent the X-axis coordinate and Y-axis coordinate of the towing trailer robot in the world coordinate system, θ i (t) is the yaw angle of the i-th trailer, 1 ≤ i ≤ N, where N is the number of trailers, and s(t) represents the pseudo-arc length trajectory. The constrained optimization problem for modeling is:[[]]

[0023]

[0024]

[0025] M p c p = b p , M s c s = b s , Mθ c θ = b θ , (2)

[0026]

[0027]

[0028] |θ j (t) - θ j-1 (t)| ≤ δθ max , (5)

[0029]

[0030]

[0031] G e (e, Info e ) ≤ 0, (8)

[0032] G c (C, e, T f , Info c ) ≤ 0. (9)

[0033] where j(t) represents the third - order derivative of the trajectory, c p is the coefficient matrix of the first two - dimensional trajectory polynomial of the trajectory, c s is the coefficient matrix of the third - dimensional trajectory polynomial of the trajectory; c θ is the coefficient matrix of the trailer trajectory polynomial in the trajectory, C = {c p , c s , c θ}, L j represents the tow bar length of the j - th trailer, M p , M s , M θ respectively represent the time - coefficient matrix of the tractor position trajectory, the time - coefficient matrix of the pseudo - arc - length trajectory, and the time - coefficient matrix of the yaw angles of all trailers; b p , b s , b θ respectively represent the constant of the tractor position polynomial equation, the constant of the pseudo - arc - length polynomial equation, and the constant of the trailer yaw - angle polynomial equation, e, T f respectively represent the end - state of the robot and the duration of the trajectory, ρ T > 0 is the weight coefficient, v0(t) represents the forward speed of the tractor, a lon (t) represents the longitudinal acceleration of the tractor, κ(t) represents the curvature of the tractor, a lat (t) represents the lateral acceleration of the tractor, v mlon is the maximum speed, amlon is the maximum longitudinal acceleration, a mlat is the maximum lateral acceleration, κ max is the maximum curvature, Info e represents the convex polygon target area information, Info c represents the signed distance field information, that is, the signed distance value corresponding to each grid.

[0034] Further, using the initial path as the initial value, solve the constrained optimization problem to obtain the final trajectory, including:

[0035] (Ⅰ) For the initial path Q with length L, where Q k (k = 1, 2, …, N l ) is the full state path point of the robot, N l is the number of path points, and the average driving speed of the towing trailer robot is v mean , take the control point spacing as L q , then traverse Q, and take the path point as the segmentation point of the piecewise polynomial trajectory every L q distance, and take the duration Take M q = N q +1 as the number of segments, N q is the number of segmentation points, take the number of segments of the piecewise polynomial trailer trajectory Ω = n k M q , n k is a constant, so as to obtain the initial value of the constrained optimization problem;

[0036] (Ⅱ) Based on the initial value of the constrained optimization problem, use the augmented Lagrangian multiplier method to iteratively solve the constrained optimization problem, where each unconstrained optimization sub-problem is solved using the quasi-Newton method or the trust region method;

[0037] (Ⅲ) Perform safety detection on the trajectory obtained by solving the constrained optimization problem, that is, take trajectory points every L c length on the generated trajectory. If there is no grid in the obstacle area among the trajectory points, then take this trajectory as the final trajectory; otherwise, the trajectory generation fails, and go to step (2).

[0038] According to the second aspect of the embodiments of the present application, there is provided a towing trailer robot trajectory generation device based on polynomials and signed distance fields, including:

[0039] A grid information calculation module, configured to use a point cloud map to divide grids, estimate the curvature and normal direction of each grid, extract obstacle information, and calculate the signed distance value corresponding to each grid;

[0040] An initial path generation module, configured to use the initial pose of the tractor-trailer robot as the root node, and based on the obstacle information, generate an initial path to reach the target area by using a multi-endpoint heuristic graph search algorithm that searches in the SE(2) space;

[0041] A trajectory optimization module, configured to combine the signed distance value corresponding to each grid, model the trajectory generation of the tractor-trailer robot as a constrained optimization problem, use the initial path as the initial value, solve the constrained optimization problem, and obtain the final trajectory.

[0042] According to a third aspect of the embodiments of the present application, there is provided a computer program product, including a computer program / instructions, which when executed by a processor, implement the method as described in the first aspect.

[0043] According to a fourth aspect of the embodiments of the present application, there is provided an electronic device, including:

[0044] One or more processors;

[0045] A memory, configured to store one or more programs;

[0046] 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 the first aspect.

[0047] According to a fifth aspect of the embodiments of the present application, there is provided a computer-readable storage medium, on which computer instructions are stored, and when the instructions are executed by a processor, the steps of the method as described in the first aspect are implemented.

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

[0049] The present application uses the signed distance field to impose obstacle avoidance constraints on the tractor-trailer robot, can avoid the loss of the solution space, and ensure optimality and safety under a certain standard; when generating the trajectory, the kinematic model of the robot, the trajectory execution time, and the energy consumption are considered, which not only avoids the trajectory obtained by optimization from being too conservative, but also reduces the risk of robot execution. After the modeling and solution of the constrained optimization problem, the trajectory is safe, smooth, and easy to execute for the tractor-trailer robot, and the algorithm has real-time performance and optimality; the generated trajectory is safe, smooth, and has good smoothness and executability for the robot.

[0050] It should be understood that the above general description and the following detailed description are only exemplary and explanatory, and cannot limit the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] The accompanying drawings herein are incorporated into the specification and form a part of the specification, showing embodiments consistent with the present application, and are used together with the specification to explain the principles of the present application.

[0052] Figure 1 It is a flowchart of a tractor-trailer robot trajectory generation method based on polynomial and signed distance field shown according to an exemplary embodiment.

[0053] Figure 2 It is a flowchart for estimating the curvature and normal direction of each grid and calculating the corresponding signed distance value;

[0054] Figure 3 It is a flowchart of a multi-endpoint heuristic graph search algorithm for searching in the SE(2) space;

[0055] Figure 4 It is a schematic diagram of the effect of a multi-endpoint heuristic graph search algorithm for searching in the SE(2) space;

[0056] Figure 5 It is a schematic diagram of the optimized trajectory using the augmented Lagrangian multiplier method.

[0057] Figure 6 It is a block diagram of a tractor-trailer robot trajectory generation device based on polynomial and signed distance field shown according to an exemplary embodiment.

[0058] Figure 7 It is a schematic diagram of an electronic device shown according to an exemplary embodiment. Detailed implementation mode

[0059] Here, the exemplary embodiments will be described in detail, and the examples are shown in the drawings. When the following description refers 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 the present application.

[0060] The terms used in the present application are only for the purpose of describing specific embodiments, and are not intended to limit the present application. The singular forms of "a", "the" and "said" used in the present 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" used herein refers to and includes any or all possible combinations of one or more of the associated listed items.

[0061] 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 each other. For example, without departing from the scope of this application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein may be interpreted as "when" or "while" or "in response to determining".

[0062] Figure 1 is a flowchart of a method for generating a tractor-trailer robot trajectory based on a polynomial and a signed distance field shown according to an exemplary embodiment, as Figure 1 shown, this method is applied to a terminal and may include the following steps:

[0063] (1) Using a point cloud map, estimate the curvature and normal direction of each grid in the grid map, extract obstacle information, and calculate the signed distance value corresponding to each grid;

[0064] (2) Taking the initial pose of the tractor-trailer robot as the root node, use a multi-endpoint heuristic graph search algorithm for searching in the SE(2) space to generate an initial path.

[0065] (3) Model the trajectory generation of the tractor-trailer robot as a constrained optimization problem, use the path generated in step (2) as the initial value, and solve the constrained optimization problem to obtain the final trajectory.

[0066] The following describes this method in detail.

[0067] (1) In this example, let the grid be p i , i = 1, 2,..., n, n = 400×400 = 160000 is the number of grids, where the map size is 40m×40m, each X-Y grid is 10 cm wide, and the position of p i corresponding in the world coordinate system is Then as Figure 2 shown, the specific steps for estimating the curvature and normal direction of the grid p i and determining whether it is an obstacle grid and calculating its signed distance value are as follows:

[0068] (Ⅰ) Let the normal direction corresponding to each grid p i be represented by , and the curvature value be represented by κ pi . Taking p i as the center, find the points in the point cloud within a spherical region that just contains the size of the tractor. In this example, the radius of the spherical region is taken as 0.5m. Let these points be represented as n iLet \(n\) be the number of points found, then their center coordinates can be calculated. Obtain the covariance matrix Thus, let \(Cov\) i The eigenvalues of are \(\lambda\) i1 \(\leq\lambda\) i2 \(\leq\lambda\) i3 , then we can get is the eigenvector corresponding to the minimum eigenvalue of \(Cov\) i .

[0069] (Ⅱ) If The third - dimensional value of is less than the threshold \(c\) normal or is greater than the threshold \(\kappa\) pmax , then set \(p\) i as an obstacle grid, otherwise set \(p\) i as a passable grid. Preferably, in this example, \(c\) normal = 0.866, \(\kappa\) pmax = 0.5, which can be set according to the actual situation in specific implementation.

[0070] (IV) For each obstacle grid, set its signed - distance value as the negative of the distance to the nearest passable grid; for each passable grid, set its signed - distance value as the distance to the nearest obstacle grid. Thus, the calculation of the signed - distance value corresponding to each grid is realized.

[0071] (2) Let the initial state of the tractor - trailer robot be \((x_0,y_0,\theta_0,\theta_1,\cdots,\theta\) N ), where represents the yaw angle of the \(j\) - th trailer, and \(x_0,y_0,\theta_0\) represent the \(X\) - axis coordinate, \(Y\) - axis coordinate, and yaw angle of the tractor - trailer robot in the world coordinate system respectively. In this example, the number of trailers \(N = 3\). The target area is represented by a convex polygon with \(M\) sides, the centroid position of the convex polygon is \((x\) f , \(y\) f ), and the direction angle corresponding to the normal direction of each side in the world coordinate system is \(\theta\) fl , \(l = 1,2,\cdots,M\), then \(M\) end - poses can be formed, and each end - pose can be expressed as \((x\) f , \(y\) f , \(\theta\) fl ). In this example, the number of sides of the convex polygon is taken as \(M = 4\). As Figure 3 shown, the specific steps of the multi - end - point heuristic graph search algorithm in the \(SE(2)\) space are as follows:

[0072] (Ⅰ) Based on the obstacle information, construct a grid map in the SE(2) space, and calculate the grid p to which the initial pose (x0, y0, θ0) of the towing trailer robot belongs according to the occupancy range of the grid in the world coordinate system. start , create a set cSet and a search node nod. s , set its total expected cost f s = 0, the current cost g s = 0, set its parent node to be empty, and add it to the set oSet; where the total expected cost is the sum of the current cost and the predicted cost.

[0073] (Ⅱ) When oSet is not empty: Take out the node nod with the minimum f s in oSet and add it to cSet. If the corresponding grid belongs to one of the grids where the unreached end poses are located, c trace back one by one from the parent node of nod start to obtain the path and store it; otherwise, if the grid to which the node belongs does not belong to any of the grids where the unreached end poses are located, but the straight-line distance to at least one unreached end pose is less than the threshold L c , then try to use the Dubins curve with the minimum turning radius of R oshot to connect the node pose with all the corresponding end poses. If the connection is successful and there are no occupied grids on the Dubins curve, obtain the path and store it. In this example, take L cmax = 10.0m, R oshot = 3.8m. cmax

[0074] (Ⅲ) If no path is obtained in (Ⅱ), then according to the pose to which nod c belongs, obtain the adjacent nodes under different speed and steering angle inputs and store them in the set nei. For each node nei i in nei, if the corresponding grid does not exist or is an obstacle grid, do not visit it; otherwise, if the grid exists and is a passable grid, calculate the expansion cost tg = g snow + w d d i + w δ |δ i | + w dδ |dδ i |, where g snow is the cost of nod c , d i is the distance between nod c and nei i , δ i is the input steering angle, dδ i is the difference in steering angle from the previous node, and w d , wδ , w dδ is a constant coefficient. Preferably, in this example, w is taken as d = 1, w δ = 0.5, w dδ = 0.1. Let nei i 's f s be f snei , g s be g snei , and the heuristic cost heu be the straight-line distance between it and the end pose. If nei i is in oSet and g snei < tg, then update the g i of nei s = tg, f s = tg + heu, and the parent node is nod c ; if nei i is in cSet and g snei < tg, then update the g i of nei s = tg, f s = tg + heu, the parent node is nod c , and remove it from cSet and re-add it to oSet; if nei i neither belongs to oSet nor cSet, then set the g i of nei s = tg, f s = tg + heu, the parent node is nod c , and include it in oSet; go to (II)

[0075] (IV) If all the paths corresponding to the end poses are found, then using the kinematic model of the towing trailer robot, at a certain time step interval, the full-state paths of all paths are obtained through numerical integration, that is, each path point in the path contains all the states of the towing trailer robot (x0, y0, θ0, θ1,..., θ N ). Subsequently, for the l-th path, the lengths Len l of all paths and the position gap Dist l between the corresponding end pose and the target area are calculated respectively, and the path cost C l is obtained by weighted summation as C l = w1 × Len l + w2 × Dist

[0076] l , where w1 > 0, w2 > 0, and w1 + w2 = 1 are weight coefficients. The path with the minimum path cost is taken as the initial path. Preferably, in this example, w1 = 0.2 and w2 = 0.8 are taken respectively.(3) The trajectory of the towing trailer robot is represented by a piecewise quintic polynomial function. Let the trajectory be expressed as traj(t) = [x(s), y(s), s(t), θ1(t), θ2(t),...,, θ N (t)] T , θ i (t) is the yaw angle of the i-th trailer (1 ≤ i ≤ N), s(t) represents the pseudo-arc length trajectory, and j(t) = traj (3) (t) represents the third-order derivative of the trajectory, which is obtained by performing trilinear interpolation in the grid map. Among them, when j = 0, the vehicle represented is the towing vehicle. Let

[0077] be the piecewise polynomial coefficient matrix, where c p is the first two-dimensional trajectory polynomial coefficient matrix of traj(t), and the number of segments is M q ; c s is the third-dimensional trajectory polynomial coefficient matrix of traj(t); c θ is the trailer trajectory polynomial coefficient matrix in traj(t), and the number of segments is Ω. Let C = {c p , c s , c θ}, among other optimization variables, T f = M q T p = ΩT θ respectively represent the end state of the robot and the duration of the trajectory. The final modeled constrained optimization problem is:

[0078]

[0079]

[0080] M p c p = b p , M s c s = b s , M θ c θ = b θ , (2)

[0081]

[0082]

[0083] |θ j (t) - θ j-1 (t)| ≤ δθ max , (5)

[0084]

[0085] G e (e, Info e ) ≤ 0, (8)

[0086] G c (C, e, T f , Info c ) ≤ 0. (9)

[0087] where ρ T > 0 is the weight coefficient. Preferably, in this example, ρ T = 50. Where L j represents the tow bar length of the j-th trailer, and M p , M s , M θ represent the time coefficient matrix of the tractor position trajectory, the time coefficient matrix of the pseudo-arc length trajectory, and the time coefficient matrix of all trailer yaw angles respectively; b p , b s , b θ represent the constants of the tractor position polynomial equation, the constants of the pseudo-arc length polynomial equation, and the constants of the trailer yaw angle polynomial equation respectively.

[0088] The objective function includes the third derivative of the trajectory and the total time. The third derivative information is to ensure the smoothness of the trajectory, and the total time is to make the optimized trajectory not too conservative. The integral of the third derivative information can be explicitly obtained.

[0089] The constraint (1) in the optimization problem represents the kinematic constraint introduced by the passive hinge of the towing trailer robot, and the constraint (2) represents the continuity constraint of the trajectory and the initial state and final state constraints, that is:

[0090]

[0091]

[0092] where the continuity constraint is the characteristic that the trajectory needs to be 4th-order continuous at the segmentation points. The constraint (3) in the optimization problem means that the time of each polynomial trajectory needs to be positive and ensure that the arc length of the trajectory is monotonically increasing. In the constraint (4), δ + is a positive constant, representing that the pseudo velocity is positive to ensure that the solution of the kinematic variables does not have the situation that the denominator approaches zero. Preferably, in this example, δ + = 1.0.

[0093] Constraints (5 - 7) in the optimization problem respectively represent the maximum bending angle between vehicles, maximum speed, maximum longitudinal acceleration, maximum lateral acceleration, and maximum curvature constraints in the robot. In this example, the maximum bending angle δθ max = 1.47 rad, the maximum speed v mlon = 1.0 m / s, the maximum longitudinal acceleration a mlon = 0.8 m / s 2 , the maximum lateral acceleration a mlat = 1.2 m / s 2 , the maximum curvature κ max = 0.26 m -1 . Other variables in the constraints are calculated as follows:

[0094]

[0095]

[0096]

[0097]

[0098] Among them, v0(t) represents the forward speed of the tractor, a lon (t) represents the longitudinal acceleration of the tractor, κ(t) represents the curvature of the tractor, and a lat (t) represents the lateral acceleration of the tractor.

[0099] Constraint (8) in the optimization problem represents the robot end - state constraint, Info e represents the convex - polygon target - region information, which is a set composed of the vertices of the convex polygon arranged in order. Constraint (9) represents the robot obstacle - avoidance constraint, Info c represents the signed - distance - field information (i.e., the signed - distance value corresponding to each grid obtained in step (1)), which is represented by a grid map and queried using trilinear interpolation. For the constraints (1), (3) - (7), (9) related to the entire trajectory in the constraint optimization problem, they are converted to imposing constraints on K points evenly taken on each of the M q segmented trajectories, where K is a constant. Preferably, in this example, K = 8.

[0100] In this step, using the path generated in step (2) as the initial value, the steps to solve for the final trajectory are as follows:

[0101] (Ⅰ) Let the path obtained in step (2) be Q, with length L, where Q k (k = 1, 2, …, N l ) is the full - state path point of the robot among them, N l is the number of path points, and the average traveling speed of the towing - trailer robot is vmean Take the control point spacing as L q , then traverse Q, and take a path point every L q distance as the segmentation point of the piecewise polynomial trajectory. Subsequently, take the duration Take M q = N q +1 as the number of segments, and N q is the number of segmentation points. Take the number of segments Ω of the piecewise polynomial trailer trajectory as n k M q , n k is a constant, so as to obtain the initial value of the constrained optimization problem described in step (3). In this example, take v mean = 1.0 m / s, L q = 2.0 m, n k = 1.8.

[0102] (II) Use the augmented Lagrangian multiplier method to iteratively solve the constrained optimization problem, and solve each unconstrained optimization sub-problem using the quasi-Newton method or the trust region method.

[0103] (III) Perform safety detection on the trajectory generated in (II), that is, take trajectory points every L c length on the generated trajectory. If there is an obstacle grid in the trajectory points, go to step (2); otherwise, the trajectory generation is successful, and return the generated trajectory. Preferably, in this example, take L c = 0.1 m.

[0104] Figure 4 is a schematic diagram of the effect of the multi-endpoint heuristic graph search algorithm for searching in the SE(2) space, Figure 5 is a schematic diagram of the trajectory after optimization using the augmented Lagrangian multiplier method. The discrete point-like objects in the figure represent point clouds, that is, the obstacle information in the environment. In Figure 4 , the four curves ①, ②, ③, and ④ respectively represent the paths of the tractor and three trailers obtained by the heuristic graph search algorithm, and the points in them represent the corresponding path points; in Figure 5 , the four curves ①, ②, ③, and ④ respectively represent the trajectories of the tractor and three trailers obtained after optimizing the constrained optimization problem. The generated trajectory is safe and smooth for the robot, and has good smoothness and executability. Figure 4 and Figure 5The hollow rectangular box in it represents the end target area. Based on polynomial representation of the tractor-trailer robot trajectory, the present invention can significantly improve the efficiency of trajectory generation and apply obstacle avoidance constraints using the signed distance field. When generating the trajectory, the kinematic model of the robot, the trajectory execution time, energy consumption, etc. are considered. While avoiding the trajectory obtained by optimization from being too conservative, the risk of robot execution is reduced. Through the modeling and solution of the constrained optimization problem, the trajectory is safe, smooth and easy to execute for the tractor-trailer robot, and the algorithm has real-time performance and optimality.

[0105] Corresponding to the foregoing embodiments of the method for generating a tractor-trailer robot trajectory based on polynomials and signed distance fields, the present application also provides embodiments of a device for generating a tractor-trailer robot trajectory based on polynomials and signed distance fields.

[0106] Figure 6 It is a block diagram of a device for generating a tractor-trailer robot trajectory based on polynomials and signed distance fields shown according to an exemplary embodiment. Referring to Figure 6 , the device may include:

[0107] A grid information calculation module 21, configured to divide grids and estimate the curvature and normal direction of each grid using a point cloud map, extract obstacle information, and calculate the signed distance value corresponding to each grid;

[0108] An initial path generation module 22, configured to use the initial pose of the tractor-trailer robot as the root node, and based on the obstacle information, use a multi-endpoint heuristic graph search algorithm for searching in the SE(2) space to generate an initial path to reach the target area;

[0109] A trajectory optimization module 23, configured to combine the signed distance value corresponding to each grid, model the trajectory generation of the tractor-trailer robot as a constrained optimization problem, use the initial path as the initial value, solve the constrained optimization problem, and obtain the final trajectory.

[0110] Regarding the device in the above embodiments, the specific manner in which each module performs operations has been described in detail in the embodiments related to the method, and will not be elaborated here.

[0111] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the descriptions in the method embodiments. The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this application. A person of ordinary skill in the art can understand and implement it without creative work.

[0112] Correspondingly, this application also provides a computer program product, including a computer program / instructions, which when executed by a processor implement the above-mentioned tractor-trailer robot trajectory generation method based on polynomials and signed distance fields.

[0113] Correspondingly, 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-mentioned tractor-trailer robot trajectory generation method based on polynomials and signed distance fields. As Figure 7 shown, it is a hardware structure diagram of any device with data processing capabilities where the tractor-trailer robot trajectory generation device provided by the embodiment of the present invention is located. In addition to Figure 7 the processors, memory, and network interfaces shown, any device with data processing capabilities where the device in the embodiment is located usually also includes other hardware according to the actual functions of the device with data processing capabilities, which will not be elaborated here.

[0114] Correspondingly, this application also provides a computer-readable storage medium, on which computer instructions are stored, and when the instructions are executed by a processor, they implement the above-mentioned tractor-trailer robot trajectory generation method based on polynomials and signed distance fields. The computer-readable storage medium can be an internal storage unit of any device with data processing capabilities 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, a Smart Media Card (SMC), an SD card, a Flash Card, etc. equipped on the device. Further, the computer-readable storage medium can also include both the internal storage unit of any device with data processing capabilities and the external storage device. The computer-readable storage medium is used to store the computer program and other programs and data required by any device with data processing capabilities, and can also be used to temporarily store the data that has been output or will be output.

[0115] Those skilled in the art will readily conceive of other embodiments of the present application after considering the specification and practicing the content disclosed herein. The present application is intended to cover any variations, uses, or adaptations of the present application, which follow the general principles of the present application and include the common general knowledge or conventional technical means in the technical field not disclosed in the present application.

Claims

1. A tractor-trailer robot trajectory generation method based on polynomials and signed distance fields, characterized in that: include: Using the point cloud map, divide the grid and estimate the curvature and normal direction of each grid, extract obstacle information and calculate the symbol distance value corresponding to each grid; Taking the initial posture of the tractor trailer robot as the root node, based on the obstacle information, and using a multi-endpoint heuristic graph search algorithm searching in SE (2) space, an initial path to the target area is generated; Combined with the signed distance value corresponding to each grid, the trajectory generation of the tractor trailer robot is modeled as a constrained optimization problem. The initial path is used as the initial value to solve the constrained optimization problem and obtain the final trajectory.

2. The method according to claim 1, characterized in that Using the point cloud map, estimate the curvature and normal direction of each grid, extract obstacle information and calculate the symbol distance value corresponding to each grid, including: For each grid p i , with p i As the center, find the point in the point cloud within a spherical area that just contains the size of the tractor n i For the number of points found, calculate the center coordinates of these points Covariance matrix Assume that the eigenvalue of the covariance matrix is i1 ≤λ i2 ≤λ i3 , then the grid p i The corresponding normal direction is i1 The corresponding eigenvector, curvature value if The third dimension value is less than the threshold c normal or Greater than the threshold κ pmax , then set p i is the obstacle grid, otherwise set p i It is a passable grid; If p i If p is an obstacle grid, set its symbol distance value to the inverse of the distance between it and the nearest passable grid; i If it is a traversable grid, set its symbol distance value to the distance between it and the nearest obstacle grid.

3. The method according to claim 1, characterized in that Taking the initial posture of the tractor trailer robot as the root node, based on the obstacle information, a multi-endpoint heuristic graph search algorithm searching in SE (2) space is used to generate an initial path to the target area, including: (I) Based on the obstacle information, a grid map of the SE(2) space is constructed, and the grid p to which the initial position (x0, y0, θ0) of the tractor trailer robot belongs is calculated according to the grid's occupied range in the world coordinate system. start , create set cSet, create search node nod s , set its total expected cost f s =0, current cost g s = 0, set its parent node to empty and put it into the set oSet; (Ⅱ) When oSet is not empty: take out f from oSet s The smallest node nod c And add cSet, if the corresponding grid belongs to one of the grids where the terminal position is not reached, then from nod c The parent nodes of the node are traced back one by one to obtain the path; if the grid to which the node belongs does not belong to any grid where the terminal position that has not been reached is located, but the straight-line distance from at least one terminal position that has not been reached is less than the threshold L oshot , then the minimum turning radius is R cmax The Dubins curve connects the node posture with all corresponding end postures. If the connection is successful and there is no occupied grid on the Dubins curve, the path is obtained; (III) If no path is obtained in (II), then according to nod c The corresponding posture obtains the adjacent nodes under different speeds and steering angles and stores them in the set nei; for each node nei in nei i If the grid does not exist or is an obstacle grid, it will not be visited; on the contrary, if the grid exists and is a passable grid, the expansion cost tg = g is calculated snow +w d d i +w δ |δ i |+w dδ |dδ i |, where g snow for nod c The price, d i for nod c with nei i The distance, δ i is the input steering angle, dδ i is the steering angle difference from the previous node, w d ,w δ ,w dδ is a constant coefficient, assuming nei i The total expected cost f s f snei , the current cost g s g snei , the inspiration cost heu is the straight-line distance between it and the end point position, if nei i In oSet and its cost is g snei <tg, then update nei i g s =tg,f s =tg+heu, parent node is nod c ; If i In cSet and g snei <tg, then update nei i g s =tg,f s =tg+heu, parent node is nod c , and remove it from cSet and re-add it to oSet; if nei i If it does not belong to oSet and does not belong to cSet, then set nei i g s =tg,f s =tg+heu, parent node is nod c , and incorporate it into oSet; go to step (Ⅱ); (IV) If all paths corresponding to the terminal postures are found, the kinematic model of the tractor trailer robot is used to obtain the full-state paths of all paths through numerical integration at certain time step intervals, and the path costs of all paths are calculated. The path with the minimum path cost is taken as the initial path.

4. The method according to claim 3, characterized in that The target area is represented by a convex polygon with M edges. The end point position (x f ,y f ,θ fl ) is represented by the centroid position of the convex polygon and the direction of the normal of each edge in the world coordinate system corresponding to the direction angle θ fl ,l=1,2,..,M.

5. The method according to claim 1, characterized in that The trajectory of the tractor trailer robot is represented by a piecewise quintic polynomial function traj(t) = [x(s), y(s), s(t), θ1(t), θ2(t), ..., θ N (t)] T , x(s), y(s) represent the X-axis coordinate and Y-axis coordinate of the tractor trailer robot in the world coordinate system, θ i (t) is the yaw angle of the i-th trailer, 1≤i≤N, N is the number of trailers, s(t) represents the pseudo arc length trajectory, and the constrained optimization problem modeled is: M p c p =b p ,M s c s =b s ,M θ c θ =b θ ,(2) |θ j (t)-θ j-1 (t)|≤δθ max , (5) G e (e,Info e )≤0, (8) G c (C,e,T f ,Info c )≤0. (9) Where j(t) represents the third-order derivative of the trajectory, c p is the first two-dimensional trajectory polynomial coefficient matrix of the trajectory, c s is the third-dimensional trajectory polynomial coefficient matrix of the trajectory; c θ is the polynomial coefficient matrix of the trailer trajectory in the trajectory, C = {c p ,c s ,c θ }, L j represents the length of the tow bar of the jth trailer, M p 、M s 、M θ They represent the time coefficient matrix of the tractor position trajectory, the time coefficient matrix of the pseudo arc length trajectory and the time coefficient matrix of the yaw angles of all trailers respectively; b p , b s , b θ denote the polynomial equation constants of the tractor position, pseudo arc length and trailer yaw angle, respectively. f They represent the robot’s terminal state and the duration of the trajectory, ρ T >0 is the weight coefficient, v0(t) represents the forward speed of the tractor, a lon (t) represents the longitudinal acceleration of the tractor, κ(t) represents the curvature of the tractor, and a lat (t) is the lateral acceleration of the tractor, v mlon is the maximum speed, a mlon is the maximum longitudinal acceleration, a mlat is the maximum lateral acceleration, κ max is the maximum curvature, Info e Represents the convex polygon target area information, Info c Represents the signed distance field information, that is, the signed distance value corresponding to each grid.

6. The method according to claim 1, characterized in that Using the initial path as the initial value, solving the constrained optimization problem to obtain the final trajectory includes: (I) For the initial path Q, the length is L, where Q k (k=1,2,…,N l ) is the robot's full state path point, N l is the number of path points, and the average speed of the tractor trailer robot is v mean , take the control point spacing as L q , then traverse Q, every L q The distance takes the path points as the segmentation points of the piecewise polynomial trajectory and takes the duration Take M q =N q +1 is the number of segments, N q is the number of segmentation points, and the segmentation number of the piecewise polynomial trailer trajectory is Ω = n k M q , n k is a constant, thus obtaining the initial value of the constrained optimization problem; (II) based on the initial value of the constrained optimization problem, using the augmented Lagrange multiplier method to iteratively solve the constrained optimization problem, wherein each unconstrained optimization subproblem is solved using the quasi-Newton method or the trust region method; (III) Perform safety check on the trajectory obtained by solving the constrained optimization problem, that is, check the generated trajectory every L c The length of the trajectory point is taken. If there is a trajectory point where there is no grid as an obstacle area, then the trajectory is taken as the final trajectory; otherwise, the trajectory generation fails and go to step (2).

7. A tractor-trailer robot trajectory generation device based on polynomials and signed distance fields, characterized in that: include: The grid information calculation module is used to divide the grid and estimate the curvature and normal direction of each grid using the point cloud map, extract obstacle information and calculate the symbol distance value corresponding to each grid; An initial path generation module is used to generate an initial path to the target area by using the initial posture of the tractor trailer robot as a root node and utilizing a multi-endpoint heuristic graph search algorithm searching in SE (2) space based on the obstacle information; The trajectory optimization module is used to combine the signed distance value corresponding to each grid, model the trajectory generation of the tractor trailer robot as a constrained optimization problem, use the initial path as the initial value, solve the constrained optimization problem, and obtain the final trajectory.

8. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instructions are executed by a processor, the method according to any one of claims 1 to 6 is implemented.

9. An electronic device, characterized in that: include: 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 method according to any one of claims 1 to 7.

10. A computer-readable storage medium having computer instructions stored thereon, characterized in that: When the instruction is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images