Vehicle parking track smooth optimization method, vehicle, electronic equipment and storage medium

Abstract

The invention provides a vehicle parking track smooth optimization method and device, electronic equipment and a storage medium. The method comprises the steps that coordinate information of obstacles around a vehicle and a reference track of the vehicle are acquired; determining a map value of each obstacle on the RCESDF map according to the coordinate information; based on the map value of each obstacle, gradient information of the collision cost is determined to serve as an obstacle constraint relation of the optimized trajectory; constructing a target function based on a parking reference trajectory and the obstacle constraint relationship; and carrying out optimization solution on the objective function by utilizing IPOPT, and taking the obtained optimal solution as a parking track after smooth optimization. According to the invention, based on the map value of each obstacle, the gradient information of the collision cost and the obstacle constraint relation as the optimized trajectory are determined, the constructed target function is solved according to the vehicle kinematics relation by using the collision avoidance optimization method, the trajectory smoothness is optimized, the solving calculation amount is reduced, and the real-time performance of the dynamic environment is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, in particular to a vehicle parking trajectory smoothing optimization method, a vehicle, an electronic device and a storage medium. BACKGROUND

[0002] In the current field of robot and automatic driving trajectory optimization, an optimization-based collision avoidance optimization (OBCA) method abstracts obstacles as convex polyhedrons, establishes boundary constraints through a hyperplane separation theorem, constructs a nonlinear optimization problem, considers a target cost function to take dynamics constraints and obstacle relationships into account, and adopts a solver to perform numerical optimization to generate a smooth trajectory. A control link takes the smooth trajectory as a reference to complete tracking control of trajectory points through a lateral and longitudinal control algorithm.

[0003] In related technologies, an optimization method using the hyperplane separation theorem to establish boundary constraints discretizes polyhedral constraints, is difficult to describe continuous non-convex obstacles, and has a high computational complexity of O(MxN) due to the product of the number of obstacle edges M and the number of path points N. If an obstacle is displaced, all hyperplanes need to be reconstructed, which is poor in real-time performance in dynamic scenarios.

[0004] In the OBCA optimization method based on collision avoidance, obstacle constraints are usually modeled by the hyperplane separation theorem, which needs to construct constraints for each boundary point of the vehicle and each plane of each obstacle, and the number of constraints is O(MxN), where M is the number of obstacle edges and N is the number of state points. A large number of dual variables (such as relaxation variables, Lagrange multipliers, etc.) are introduced, which leads to a dramatic increase in the dimension of the optimization problem and a high time consumption of seconds (1-2s / iteration) for solving, resulting in a large amount of calculation and being unable to meet the real-time performance requirements in dynamic environments.

[0005] Therefore, how to reduce the amount of calculation and improve the real-time performance of the optimization based on collision avoidance to meet the real-time performance requirements in dynamic environments is a problem to be solved at present. SUMMARY

[0006] The present application provides a vehicle parking trajectory smoothing optimization method, a vehicle, an electronic device and a storage medium to at least solve the problem in related technologies that a large number of dual variables are introduced due to the need to construct constraints for each boundary point of the vehicle and each plane of each obstacle, which leads to a dramatic increase in the dimension of the optimization problem and a large amount of calculation for solving, and poor real-time performance in dynamic environments. The technical solutions of the present application are as follows: According to a first aspect of an embodiment of the present application, a vehicle parking trajectory smoothing optimization method is provided, comprising: Obtain coordinate information of obstacles around a vehicle and a reference trajectory of the vehicle, the coordinate information being used to represent positions of the obstacles in a coordinate system constructed with the vehicle as a reference; Determine, according to the coordinate information, a map value of each obstacle on a robot center Euclidean signed distance field (RC ESDF) map, the map value representing a nearest distance from the obstacle corresponding to the coordinate information to a contour of the vehicle; Determine gradient information of a collision cost based on the map value of each obstacle, and use the gradient information as an obstacle constraint relationship for optimizing a trajectory; Construct a target function based on the parking reference trajectory and the obstacle constraint relationship; Optimize and solve the target function by using an IPOPT (Interior Point OPTimizer) and use an optimal solution obtained as a smooth optimized parking trajectory.

[0007] Optionally, the determining, based on the map value of each obstacle, of gradient information of a collision cost and the use of the gradient information as an obstacle constraint relationship for optimizing a trajectory include: Obtain a global pose of the vehicle in the coordinate system, the global pose including a position and an orientation of the vehicle; Calculate, based on the position and the orientation of the vehicle and the map value of each obstacle, gradient information of a global pose collision cost of the vehicle with respect to the map value of each obstacle by using a collision cost function; Use the gradient information of the collision cost as the obstacle constraint relationship for optimizing a trajectory to guide the vehicle away from the obstacles in trajectory optimization.

[0008] Optionally, the constructing, based on the parking reference trajectory and the obstacle constraint relationship, of a target function includes: Determine state variables and control variables of the parking reference trajectory, the state variables including coordinate positions, velocities, and heading angles of the vehicle, and the control variables including front wheel steering angles and accelerations of the vehicle; Construct, based on the state variables and the control variables of the reference trajectory and the obstacle constraint relationship, a target function by using an OBCA (Optimal Collision Avoidance) method, the target function including a sum of a kinematic cost function and a collision cost function and a constraint condition.

[0009] Optionally, the optimizing and solving the target function by using an IPOPT (Interior Point OPTimizer) and using an optimal solution obtained as a smooth optimized parking trajectory include: Determine a Jacobian matrix of the target function; The Jacobian matrix is solved by using a nonlinear optimization problem solver IPOPT, and an optimal solution obtained is taken as a smoothed optimized parking trajectory.

[0010] Optionally, the determining the Jacobian matrix of the objective function comprises: calculating the Jacobian matrix of the kinematic cost function and the Jacobian matrix of the collision cost function in the objective function; taking the Jacobian matrix of the kinematic cost function and the Jacobian matrix of the collision cost function as the Jacobian matrix of the objective function.

[0011] Optionally, the determining the map value of each obstacle on the robot-centered Euclidean signed distance field (RC_ESDF) map according to the coordinate information comprises: mapping the position of each obstacle to a preset grid on a pre-constructed RC_ESDF map according to the coordinate information, and obtaining a corresponding grid index; calculating the map value of each obstacle based on the grid index.

[0012] Optionally, the calculating the map value of each obstacle based on the grid index comprises: if the grid index is within the grid range on the RC_ESDF map and the obstacle is located at the center position of the grid, then directly reading the distance value stored on the grid as the map value of the corresponding obstacle; if the grid index is within the grid range on the RC_ESDF map and the obstacle is located at a non-center position in the grid, then calculating the corresponding map value of the corresponding obstacle by bilinear interpolation and combining the distance values of all or part of the adjacent grids around the corresponding obstacle.

[0013] Optionally, the obtaining the coordinate information of the obstacles around the vehicle and the reference trajectory comprises: obtaining the global pose of the vehicle when parking, the global pose comprising: a coordinate point and an orientation, and the coordinate information of the obstacles around the vehicle and the reference trajectory of the vehicle; converting the coordinate information of each obstacle to coordinate information in the coordinate system of the vehicle itself based on the global coordinate point and the orientation of the vehicle when parking, to obtain the converted coordinate information of the obstacles.

[0014] According to a second aspect of the embodiments of the present application, a vehicle parking trajectory smoothing optimization device is provided, comprising: an obtaining module, configured to obtain the coordinate information of the obstacles around the vehicle and the reference trajectory of the vehicle, the coordinate information being used to represent the position of the obstacles in a coordinate system constructed with the vehicle as a reference; The first determining module is configured to determine a map value of each obstacle on a robot center Euclidean signed distance field (RC_ESDF) map according to the coordinate information, where the map value represents a nearest distance from the obstacle corresponding to the coordinate information to a contour of the vehicle; The second determining module is configured to determine gradient information of a collision cost based on the map value of each obstacle, and use the gradient information as an obstacle constraint relationship for trajectory optimization. The constructing module is configured to construct an objective function based on the parking reference trajectory and the obstacle constraint relationship. The optimization module is configured to use an interior point non-linear optimization (IPOPT) solver to optimize and solve the objective function, and use an optimal solution obtained as a smooth optimized parking trajectory.

[0015] Optionally, the second determining module comprises: The pose obtaining module is configured to obtain a global pose of the vehicle in the coordinate system, where the global pose comprises a position and an orientation of the vehicle. The obstacle gradient calculating module is configured to use a collision cost function to calculate gradient information of a collision cost of the global pose of the vehicle with respect to the map value of each obstacle based on the position and the orientation of the vehicle and the map value of each obstacle. The obstacle constraint relationship determining module is configured to use the gradient information of the collision cost as an obstacle constraint relationship for trajectory optimization, so as to guide the vehicle to move away from the obstacles in trajectory optimization.

[0016] Optionally, the constructing module comprises: The variable determining module is configured to determine state variables and control variables of the parking reference trajectory, where the state variables comprise a coordinate position, a speed and a heading angle of the vehicle, and the control variables comprise a front wheel steering angle and an acceleration of the vehicle. The objective function constructing module is configured to use an obstacle collision avoidance (OBCA) method to construct an objective function based on the state variables, the control variables of the reference trajectory and the obstacle constraint relationship, where the objective function comprises a sum of a kinematic cost function and a collision cost function, and a constraint condition.

[0017] Optionally, the optimization module comprises: The Jacobian matrix determining module is configured to determine a Jacobian matrix of the objective function. The optimization solving module is configured to use the IPOPT solver to optimize and solve the Jacobian matrix, and use an optimal solution obtained as a smooth optimized parking trajectory.

[0018] Optionally, the matrix determining module comprises: a Jacobian matrix determination module configured to calculate a Jacobian matrix of the kinematic cost function and a Jacobian matrix of the collision cost function in the target function; a summation module configured to sum the Jacobian matrix of the kinematic cost function and the Jacobian matrix of the collision cost function as the Jacobian matrix of the target function.

[0019] Optionally, the first determination module comprises: a mapping module configured to map the position of each obstacle to a preset grid on a robot-centered Euclidean signed distance field (RC_ESDF) map pre-constructed according to the coordinate information, and obtain a corresponding grid index; a map value calculation module configured to calculate a map value of each obstacle based on the grid index.

[0020] Optionally, the map value calculation module comprises: a first map value calculation module configured to, when the grid index is within the grid range of the RC_ESDF map and the obstacle is located at the center position of the grid, directly read a distance value stored on the grid as the map value of the corresponding obstacle; a first map value calculation module configured to, when the grid index is within the grid range of the RC_ESDF map and the obstacle is located at a non-central position in the grid, calculate a corresponding map value of the corresponding obstacle by bilinear interpolation in combination with distance values of all or part of adjacent grids around the corresponding obstacle.

[0021] Optionally, the obtaining module comprises: a parameter obtaining module configured to obtain a global pose of the vehicle, the global pose comprising: a position (i.e., a coordinate point) and an orientation of the vehicle, and coordinate information of obstacles around the vehicle and a reference trajectory of the vehicle; a conversion module configured to convert the coordinate information of each obstacle to a vehicle self-coordinate system based on the position and the orientation of the vehicle when the vehicle is parking, to obtain converted coordinate information of the obstacles.

[0022] According to a third aspect of the embodiments of the present application, an electronic device is provided, comprising: a processor, a memory; and a program or instructions stored on the memory and executable on the processor, the program or instructions being executed by the processor to implement the steps of the vehicle parking trajectory smoothing optimization method as described above.

[0023] According to a fourth aspect of the embodiments of the present application, a vehicle is provided, comprising the electronic device as described above.

[0024] According to a fifth aspect of the embodiments of the present application, a readable storage medium is provided, and the readable storage medium stores programs or instructions, and the programs or instructions are executed by a processor to implement the steps of the vehicle parking trajectory smoothing optimization method.

[0025] According to a sixth aspect of the embodiments of the present application, a computer program product is provided, and the computer program product includes computer programs or instructions, and the computer programs or instructions are executed by a processor of an electronic device to implement the steps of the vehicle parking trajectory smoothing optimization method.

[0026] The embodiments of the present application at least have the following beneficial effects: In the embodiments of the present application, the coordinate information of the obstacles around the vehicle and the reference trajectory of the vehicle are obtained, and the coordinate information is used to represent the positions of the obstacles in a coordinate system constructed with the vehicle as a reference; according to the coordinate information, the map value of each obstacle on a robot-centric Euclidean signed distance field (RC_ESDF) map is determined, and the map value represents the nearest distance from the obstacle corresponding to the coordinate information to the contour of the vehicle; based on the map value of each obstacle, the gradient information of the collision cost is determined as the obstacle constraint relationship of the optimized trajectory; based on the parking reference trajectory and the obstacle constraint relationship, a target function is constructed; the target function is optimized and solved by using an interior point non-linear optimization problem solver (IPOPT), and the optimal solution obtained is used as the smoothed and optimized parking trajectory. That is, in the embodiments of the present application, the map value of the coordinate information of each obstacle around the vehicle during parking is determined on the RC_ESDF map, the gradient information of the obstacle is determined based on the map value of each obstacle as the obstacle constraint relationship, the target function is constructed by using the collision avoidance optimization method, the trajectory smoothing is optimized, the calculation amount is reduced, and the problem that the traditional collision avoidance optimization method relies on the hyperplane to separate the obstacle constraint, resulting in long solving time and poor real-time performance in a dynamic environment is solved.

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

[0028] The accompanying drawings, which are incorporated into and constitute a part of the specification, illustrate preferred embodiments of the present application and, together with the description, serve to explain the principles of the present application, and are not intended to limit the present application in any way. In order to more clearly illustrate the technical solutions in the embodiments of the present application or in the prior art, the accompanying drawings needed to be used in the description of the embodiments or the prior art will be briefly introduced. Obviously, the accompanying drawings in the following description are some embodiments of the present application, and those skilled in the art can obtain other drawings according to these drawings without creative effort.

[0029] Figure 1 is a flowchart of a vehicle parking trajectory smoothing optimization method provided by an embodiment of the present application.

[0030] Figure 2 is a schematic diagram of an RC_ESDF map provided by an embodiment of the present application.

[0031] Figure 3 is a block diagram of a vehicle parking trajectory smoothing optimization device provided by an embodiment of the present application.

[0032] Figure 4 is a block diagram of a first determination module provided by an embodiment of the present application.

[0033] Figure 5 is a block diagram of a map value calculation module provided by an embodiment of the present application.

[0034] Figure 6 is a block diagram of an electronic device provided by an embodiment of the present application.

[0035] Figure 7 is a block diagram of a device for vehicle parking trajectory optimization provided by an embodiment of the present application. DETAILED DESCRIPTION

[0036] In order for those skilled in the art to better understand the technical solutions of the present application, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the accompanying drawings.

[0037] It should be noted that the terms "first", "second", and the like in the specification and claims of the present application and the above-mentioned accompanying drawings are used to distinguish similar objects, and do not necessarily indicate a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application. Rather, they are merely examples of devices and methods consistent with some aspects of the present application as detailed in the appended claims.

[0038] Figure 1 is a flowchart of a vehicle parking trajectory smoothing optimization method provided by an embodiment of the present application, as shown in the figure, the method comprises the following steps: Figure 1 Step 101: Obtain coordinate information of obstacles around a vehicle and a reference trajectory of the vehicle, the coordinate information being used to represent positions of the obstacles in a coordinate system constructed with the vehicle as a reference.

[0039] Step 102: According to the coordinate information, determine a map value of each obstacle on a robot-centric Euclidean signed distance field (RC_ESDF) map, the map value representing a nearest distance from the obstacle corresponding to the coordinate information to a contour of the vehicle.

[0040] Step 103: Based on the map value of each obstacle, determine gradient information of a collision cost, and take the gradient information as an obstacle constraint relationship of an optimized trajectory.

[0041] Step 104: Based on the parking reference trajectory and the obstacle constraint relationship, construct an objective function.

[0042] Step 105: Use an interior point non-linear optimization problem solver (IPOPT) to optimize and solve the objective function, and take an optimal solution obtained as a smoothed and optimized parking trajectory.

[0043] In an embodiment of the present application, a map value of coordinate information of each obstacle around a vehicle is determined on an RC_ESDF map, gradient information of an obstacle is determined based on the map value of each obstacle, taken as an obstacle constraint relationship, an objective function is constructed using a collision avoidance optimization method, and trajectory smoothing is optimized, which reduces the amount of calculation for solving, solves the problem that a traditional collision avoidance optimization method relies on a hyperplane to separate obstacle constraints, resulting in long time consumption for solving and poor real-time performance in a dynamic environment, and improves real-time performance of an optimization method based on collision avoidance.

[0044] The vehicle parking trajectory smoothing optimization method described in the present application can be applied to a vehicle end, a cloud end, an automatic driving control system, etc., without limitation, and a vehicle end implementation device thereof can be an electronic device such as a vehicle terminal, a vehicle control platform, and an industrial computer, the cloud end can be a server, a server cluster, or a server providing cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, intermediate services, domain name services, security services, content distribution networks, or big data and artificial intelligence platforms, without limitation.

[0045] The following will be described in combination with Figure 1 ​The specific implementation steps of the vehicle parking trajectory smoothing optimization method provided by the embodiment of the application are described in detail.

[0046] In step 101, coordinate information of an obstacle around a vehicle and a reference trajectory of the vehicle are acquired, and the coordinate information is used to represent a position of the obstacle in a coordinate system constructed with the vehicle as a reference.

[0047] In this step, the global coordinate point and orientation of the vehicle during the parking process of the vehicle can be detected in real time by a camera, a laser radar, a radar or an ultrasonic sensor on the vehicle, and the obstacle information around the vehicle and the reference trajectory (such as a planned trajectory of the vehicle during parking) of the vehicle are acquired in real time. Based on the global coordinate point and orientation of the vehicle, the coordinate information of each obstacle is converted into coordinate information in the coordinate system of the vehicle itself to obtain the coordinate information of the converted obstacle.

[0048] That is, the global pose of the current vehicle in the coordinate system is acquired first, and the global pose includes a position point (i.e., a global coordinate point) P of the vehicle and an orientation The obstacle in the surrounding environment is converted into the coordinate system of the vehicle itself to obtain the converted obstacle Specifically, the conversion can be performed according to the following formula: , wherein, is a rotation matrix, P represents the global coordinate point of the vehicle, represents the orientation of the vehicle, represents the coordinate value of the obstacle around the vehicle in the global coordinate system, represents the coordinate value of the obstacle in the coordinate system of the vehicle itself.

[0049] The specific implementation process is well known to those skilled in the art, and will not be described here.

[0050] In this step, the vehicle coordinate system and the RC_ESDF map are in a relative mapping correlation relationship, the vehicle coordinate system is constructed with the real-time position of the vehicle on the RC_ESDF map as a coordinate origin, and precise correspondence between the vehicle pose and the map grid is achieved. The coordinate system is constructed with the position of the vehicle on the RC_ESDF map as an origin.

[0051] ​That is, the vehicle coordinate system is dynamically mapped to the RC_ESDF map, which is a preset global grid map with grid indexes corresponding to actual spatial positions one by one; the vehicle coordinate system is a vehicle-mounted local coordinate system, which is constructed with the real-time spatial position of the vehicle on the RC_ESDF map as the coordinate origin and the direction of vehicle travel as the positive direction of the horizontal axis, for realizing coordinate conversion and spatial matching of vehicle local perception data and RC_ESDF global map data.

[0052] In step 102, according to the coordinate information, the map value of each obstacle on the robot center Euclidean signed distance field RC_ESDF map is determined, which represents the nearest distance from the obstacle corresponding to the coordinate information to the contour of the vehicle.

[0053] Before performing this step, first, the RC_ESDF map is constructed in advance. That is, the nearest distance (Euclidean distance) from each spatial point in the parking range to the vehicle contour is calculated based on the own coordinate system, with a positive value representing outside the vehicle contour and a negative value representing inside the vehicle contour, thereby generating a fixed-size grid distance field. This process can be completed offline and only needs to be constructed once. The steps of constructing the RC_ESDF map include: 1) in the robot coordinate system, set the size of the bounding box and the grid resolution, calculate the grid size to create the RC_ESDF map; 2) calculate the minimum Euclidean distance to the robot surface at each grid point on the RC_ESDF; use the ray method to determine whether the point is inside the robot, and the distance is negative inside, and the point outside is positive; 3) convert the obstacle to the RC_ESDF map to obtain the grid index on the map; 4) use the grid index to determine whether the obstacle is inside the robot to obtain the value di corresponding to the index; di < 0 inside the robot; otherwise, di > 0. It should be noted that the specific construction process is well known to those skilled in the art, and will not be described here. The RC_ESDF map constructed in this embodiment is shown in Figure 2 Figure 2 A schematic diagram of an RC_ESDF map provided by the embodiment of the application. The RC-ESDF map is used in the embodiment of the application to realize fast trajectory optimization, especially better obstacle avoidance capability in dense environment.

[0054] Then, according to the coordinate information, the position of each obstacle is mapped to the preset grid on the robot center Euclidean signed distance field RC_ESDF map constructed in advance, and the corresponding grid index is obtained; and the map value of each obstacle is calculated based on the grid index.

[0055] The calculation of the map value of each obstacle based on the grid index includes:​ Firstly, it is judged whether the grid index is within the grid range on the RC ESDF map. If not, it is an invalid value and is discarded. If yes, it is necessary to continue to judge whether the obstacle is located at the center position of the grid. If it is located at the center position of the grid, the distance value stored on the grid is directly read as the map value corresponding to the obstacle, which is the minimum Euclidean distance between the obstacle and the surface of the robot on the RC ESDF map. If it is located at a non-center position of the grid, the map value corresponding to the obstacle needs to be calculated through bilinear interpolation in combination with the distance values of all or part of the adjacent grids around the corresponding obstacle. The specific calculation process is well known to those skilled in the art and will not be repeated here.

[0056] In the embodiment, the RC ESDF value di of each obstacle is calculated based on the distance between the obstacle and the vehicle contour. The RC ESDF value di of each obstacle is calculated based on the distance between the obstacle and the vehicle contour. If not, the distance value in the continuous space is obtained through bilinear interpolation to avoid discretization error.

[0057] In step 103, the gradient information of the collision cost is determined based on the map value of each obstacle, and the gradient information is taken as the obstacle constraint relationship of the optimized trajectory.

[0058] In this step, the gradient information of the collision cost of the vehicle global pose is determined based on the global coordinate point and orientation of the vehicle and the map value of each obstacle. Specifically, the global pose of the vehicle in the coordinate system is obtained, which includes the position and orientation of the vehicle. Based on the position and orientation of the vehicle and the map value of each obstacle, the gradient information of the map value of each obstacle to the global pose collision cost of the vehicle is calculated by using the collision cost function.

[0059] The obstacle constraint relationship refers to the quantitative constraint rule imposed on the vehicle global pose sequence, control input and motion trajectory to avoid collision between the vehicle and the obstacle. Generally, the geometric position and gradient information of the obstacle are converted into mathematical constraint conditions of the optimization problem.

[0060] That is, in this step, in order to guide the vehicle away from the obstacle in trajectory optimization, the gradient information of the collision cost of the global pose of the vehicle is calculated. J c The gradient information of the global pose of the vehicle is calculated, wherein the collision cost function J c is as follows:

[0061]

[0062]

[0063] wherein, F c is a differentiable function, P k represents the xy value of the global coordinate point of the vehicle at time k, k represents the heading of the vehicle under the global at time k, is the global pose of the vehicle RC_ESDF value (i.e. map value) of all obstacles at time k, Md is the number of obstacles, di is the map value (i.e. distance) of the i-th obstacle in the RC_ESDF map; ω is the weight corresponding to the attribute of the obstacle, usually, if the obstacle is a static obstacle, ω = 1.0, if the obstacle is a dynamic obstacle, ω = 1.2, of course, the value of ω is not limited to this in specific application, and can be adaptively adjusted according to the actual situation, and the embodiment is not limited.

[0064] In the embodiment, the gradient information of the collision cost is taken as the obstacle constraint relationship of the optimized trajectory, so as to guide the vehicle to move away from the obstacle in the trajectory optimization.

[0065] In the embodiment, the distance from the obstacle is dynamically adjusted through the obstacle constraint relationship, so as to improve the adaptability of the algorithm to the dynamic obstacle.

[0066] In step 104, a target function is constructed based on the parking reference trajectory and the obstacle constraint relationship.

[0067] In this step, the state variable and the control variable of the parking reference trajectory are acquired first, the state variable includes the coordinate position, the speed and the heading angle of the vehicle, and the control variable includes the front wheel steering angle and the acceleration of the vehicle; and based on the state variable and the control variable of the reference trajectory and the obstacle constraint relationship, a target function is constructed by using the collision avoidance optimization OBCA method, the target function includes the sum of the kinematics cost function and the collision cost function, and the constraint condition.

[0068] That is to say, in this step, the OBCA method (which is an optimization-based path smoothing algorithm) can be used to predict the state by using the vehicle kinematics model based on the model predictive control (MPC) framework, which specifically includes: 1) constructing a target function First, the state variable X k and the control variable U k of the parking reference trajectory are determined: State variable:

[0069] wherein, denotes the lateral position x of the vehicle, denotes the longitudinal position y of the vehicle, denotes the speed of the vehicle, denotes the heading angle of the vehicle, T denotes the matrix transpose.

[0070] Control variables (i.e. control inputs):

[0071] wherein, denotes the front wheel steering angle of the vehicle, denotes the acceleration.

[0072] Secondly, based on the state variables of the parking reference trajectory, the control variables and the obstacle constraint relationship, a target function of the collision avoidance optimization OBCA is constructed according to a vehicle kinematics model and a kinematics cost function.

[0073] wherein, the vehicle kinematics model is:

[0074] wherein, L is the wheelbase, is the sampling time, denotes the lateral position x of the vehicle, denotes the longitudinal position y of the vehicle, denotes the speed of the vehicle, denotes the heading angle of the vehicle, denotes the front wheel steering angle of the vehicle, denotes the acceleration.

[0075] The kinematics cost function is:

[0076] wherein, Q, R, S and P respectively denote the weight matrix, N denotes the time domain, k = 0, k = 1, k = 2, etc. denote specific time, the time domain contains all time, X k denotes the state variable of the reference trajectory at k time, U k denotes the control variable of the reference trajectory at k time, X k+1 denotes the state variable of the reference trajectory at k+1 time, U k+1 denotes the control variable of the reference trajectory at k+1 time, X end denotes the state variable of the reference trajectory at the end, X N denotes the state variable derived from X0 to the end time by kinematics formula.

[0077] Finally, the target function is constructed by using the collision avoidance optimization OBCA method, and the target function includes the sum of a kinematic cost function Js and a collision cost function Jc, and the formula is:

[0078] The constraint condition (i.e. obstacle constraint relationship) is:

[0079] In this step, the RC_ESDF is used to construct the collision constraint, the demand for dual variables of the traditional OBCA algorithm is eliminated, the ESDF gradient is used to guide the obstacle avoidance direction of the optimizer, and thus the real-time performance of the collision avoidance optimization algorithm is improved.

[0080] In step 105, the target function is optimized and solved by using the nonlinear optimization problem solver IPOPT, and the obtained optimal solution is used as the smoothed optimized trajectory. In the parking scene, the smoothed optimized trajectory can be used as the parking trajectory.

[0081] In this step, the Jacobian matrix of the target function is determined, and the Jacobian matrix is optimized and solved by using the nonlinear optimization problem solver IPOPT, and the optimal solution is used as the smoothed optimized parking trajectory.

[0082] The determination of the Jacobian matrix of the target function includes: calculating the Jacobian matrix of the kinematic cost function and the Jacobian matrix of the collision cost function in the target function; and taking the Jacobian matrix of the kinematic cost function and the Jacobian matrix of the collision cost function as the Jacobian matrix of the target function.

[0083] In this embodiment, the Jacobian matrix is a matrix arranged in a certain way by using the first-order partial derivative, and the determinant of the matrix is called the Jacobian determinant. That is, the Jacobian matrix reflects the optimal linear approximation of a differentiable equation to a given point. Therefore, the Jacobian matrix is similar to the derivative of a multivariate function. Suppose that F: Rn→ Rm is a function from the Euclidean n-dimensional space to the Euclidean m-dimensional space. The function is composed of m real functions: y1(x1, …, xn), …, ym(x1, …, xn). The partial derivatives (if any) of these functions can form an m-column n-row matrix (mxn), which is the Jacobian matrix.

[0084] On the basis of understanding the Jacobian matrix, in this embodiment, the calculation of the Jacobian matrix of the target function includes: 1) Calculate the kinematic cost function Jacobian matrix.

[0085] In this embodiment, the optimization target function is to minimize the target function J minand the objective function is guided by the Jacobian matrix in the direction of reduction. Each row of the Jacobian matrix is the partial derivative of the objective function with respect to the optimization variable.

[0086] where the state variable is the partial derivative of:

[0087] The end point Xend penalty term is:

[0088] The control variable is the partial derivative of:

[0089] The control rate of change U k The penalty term is:

[0090] where the indicates the partial derivative, and Q, R, S and P represent weight matrices respectively.

[0091] 2) Calculate the collision cost function Jacobian matrix, which is implemented according to the following formula:

[0092]

[0093]

[0094] In the formula, the meanings of various parameters are described above and will not be repeated here.

[0095] That is, in the embodiment, to calculate the minimum value of the collision cost function, the function F c is differentiable, and the gradient of the function F c is calculated. In the embodiment, the AABB algorithm is used as an example, that is, the AABB algorithm is used to define a rectangular region according to the position of the vehicle in the world coordinate system. The obstacle in the AABB bounding box can be converted to the point in the vehicle body coordinate system, is a rotation matrix, which is implemented according to the following formula:

[0096] When the global pose of the vehicle is , the RC_ESDF value of the i-th obstacle is where The gradient of P is the partial derivative of P with respect to and where P is the partial derivative of P with respect to k represents the xy value of the vehicle global coordinate point at time k, k represents the orientation of the vehicle under the global coordinate at time k, which is detailed in the following formula:

[0097]

[0098] where, is the rotation matrix, and T represents the matrix transpose.

[0099] Similarly, the function F c The gradient can be expressed as:

[0100] 3) Finally, the kinematic model, gradient, and constraints are constructed into the data structure required by the IPOPT solver library, and the IPOPT solver library is used for optimization solving to obtain the optimal solution, and the obtained optimal solution is used as the smoothed optimized parking trajectory.

[0101] where IPopt (Interior Point OPTimizer) is a software library for solving large-scale nonlinear optimization problems, especially problems under constraints, and is widely used in fields that require efficient optimization problem solving. IPopt is based on its Interior Point Method algorithm, which is particularly suitable for handling problems with a large number of inequality constraints and has advantages in terms of solving speed and accuracy. The solver also provides various customization options, such as linear search strategies, gradient calculation methods, and the selection of quadratic programming solvers, allowing users to customize the solving strategy according to the specific problem in order to effectively solve the optimization problem.

[0102] In the embodiment of the present application, the coordinate information of the obstacles around the vehicle and the reference trajectory of the vehicle are obtained, and the coordinate information is used to represent the position of the obstacles in the coordinate system constructed based on the vehicle; according to the coordinate information, the map value of the coordinate information of each obstacle on the robot-centric Euclidean signed distance field (RC_ESDF) map is determined; based on the map value of each obstacle, the obstacle gradient information is determined as the obstacle constraint relationship; based on the parking reference trajectory and the obstacle constraint relationship, a target function is constructed; the target function is optimized and solved by using an IPOPT (nonlinear optimization problem solver), and the optimal solution obtained is used as the smoothed and optimized parking trajectory. That is, in the embodiment of the present application, the map value of each obstacle around the vehicle is determined on the RC_ESDF map, the gradient information of the collision cost is determined based on the map value of each obstacle as the obstacle constraint relationship, the target function is constructed according to the kinematic relationship of the vehicle by using the collision avoidance optimization method, the trajectory smoothness is optimized, the calculation amount is reduced, and the problem that the traditional collision avoidance optimization method relies on the hyperplane to separate the obstacle constraint, resulting in long solving time and poor real-time performance in dynamic environment is solved.

[0103] Optionally, in another embodiment, the method can further include: controlling the vehicle to park according to the smoothed and optimized parking trajectory.

[0104] In this embodiment, during the process of parking according to the smoothed and optimized parking trajectory, it is judged whether the parking task needs to be ended, if yes, the parking task is ended, otherwise, the above steps are returned to and the parking trajectory is replanned.

[0105] In the embodiment of the present application, the RC_ESDF gradient driven collision constraint reconstruction is used to provide the distance from any position to the nearest obstacle and the gradient direction, eliminate the demand for dual variables, use the ESDF gradient direction to directly guide the obstacle avoidance direction of the optimizer, and accelerate the convergence.

[0106] In the embodiment of the present application, the RC_ESDF-OBCA hierarchical optimization architecture is used, the RC_ESDF processes geometric obstacle avoidance, the OBCA focuses on kinematic optimization, and the division and cooperation improve the efficiency and adapt to the mobile obstacles.

[0107] In the embodiment of the present application, the RC_ESDF construction based on multi-modal perception fusion is used, the dynamic obstacle attributes are combined, the safety distance is dynamically adjusted, and the adaptability to the dynamic obstacles is improved.

[0108] It should be noted that, for the sake of simplicity, the method embodiments are all described as a series of actions. However, those skilled in the art should understand that this disclosure is not limited to the described order of actions, because according to this application, some steps can be performed in other orders or simultaneously. Secondly, those skilled in the art should also understand that the embodiments described in the specification are all preferred embodiments, and the actions involved are not necessarily essential to this application.

[0109] Figure 3 This is a block diagram of a vehicle parking trajectory smoothing optimization device provided in an embodiment of this application. The device includes: an acquisition module 301, a first determination module 302, a second determination module 303, a construction module 304, and an optimization module 305, wherein... The acquisition module 301 is used to acquire the coordinate information of obstacles around the vehicle and the reference trajectory of the vehicle. The coordinate information is used to represent the position of the obstacle in a coordinate system constructed with the vehicle as the reference. The first determining module 302 is used to determine the map value of each obstacle on the robot center Euclidean symbol distance field RC_ESDF map according to the coordinate information, wherein the map value represents the shortest distance from the obstacle corresponding to the coordinate information to the outline of the vehicle. The second determining module 303 is used to determine the gradient information of the collision cost based on the map value of each obstacle, and use the gradient information as the obstacle constraint relationship for optimizing the trajectory. Construction module 304 is used to construct an objective function based on the parking reference trajectory and the obstacle constraint relationship; The optimization module 305 is used to optimize the objective function using the nonlinear optimization problem solver IPOPT, and the obtained optimal solution is used as the smoothed and optimized parking trajectory.

[0110] Optionally, in another embodiment, based on the above embodiments, the first determining module 302 includes: a mapping module 401 and a map value calculation module 402, the structural block diagram of which is shown below. Figure 4 As shown, where, The mapping module 401 is used to map the position of each obstacle to a preset grid on a pre-constructed robot center Euclidean symbol distance field RC_ESDF map according to the coordinate information, and obtain the corresponding grid index. The map value calculation module 402 is used to calculate the map value of each obstacle based on the grid index.

[0111] Optionally, in another embodiment, which is based on the above-mentioned embodiments, the map value calculation module comprises: a first map value calculation module, configured to determine, when the grid index is within the grid range of the RC_ESDF map and the obstacle is located at the center position of the grid, directly read the distance value stored on the grid as the map value of the corresponding obstacle. The first map value calculation module is configured to determine, when the grid index is within the grid range of the RC_ESDF map and the obstacle is located at a non-central position in the grid, calculate the map value of the corresponding obstacle by bilinear interpolation and in combination with the distance values of all (such as eight) or part (such as four) adjacent grids around the corresponding obstacle.

[0112] Optionally, the acquisition module comprises a parameter acquisition module 501 and a conversion module 502, and a structural block diagram thereof is as shown in Figure 5 The parameter acquisition module 501 is configured to acquire the global pose of the vehicle when parking, wherein the global pose comprises the position (i.e. coordinate point) and orientation of the vehicle, and the coordinate information of the obstacles around the vehicle and the reference trajectory of the vehicle. The conversion module 502 is configured to convert the coordinate information of each obstacle to the vehicle's own coordinate system based on the position and orientation of the vehicle when parking, to obtain the coordinate information of the converted obstacle. Optionally, in another embodiment, which is based on the above-mentioned embodiments, the second determination module comprises:

[0113] A pose acquisition module, configured to acquire the global pose of the vehicle in the coordinate system, wherein the global pose comprises the position and orientation of the vehicle. An obstacle gradient calculation module, configured to calculate, by using a collision cost function, gradient information of the collision cost of the global pose of the vehicle with respect to the map value of each obstacle, based on the position and orientation of the vehicle and the map value of each obstacle. An obstacle constraint relationship determination module, configured to use the gradient information of the collision cost as an obstacle constraint relationship of the optimized trajectory, to guide the vehicle to move away from the obstacle in trajectory optimization. Optionally, in another embodiment, which is based on the above-mentioned embodiments, the construction module comprises:

[0114] A variable determination module, configured to determine the state variable and the control variable of the parking reference trajectory, wherein the state variable comprises the coordinate position, speed and heading angle of the vehicle, and the control variable comprises the front wheel steering and acceleration of the vehicle. ​The target function construction module is configured to construct a target function based on state variables and control variables of the parking reference trajectory and the obstacle constraint relationship by using an obstacle collision avoidance (OBCA) method, and the target function comprises a sum of a kinematic cost function and a collision cost function, and a constraint condition.

[0115] Optionally, in another embodiment, the optimization module comprises: The Jacobian matrix determination module is configured to determine a Jacobian matrix of the target function. The optimization solution module is configured to perform optimization solution on the Jacobian matrix by using an IPOPT (an interior point optimizer) and obtain an optimal solution as the smoothed parking trajectory.

[0116] Optionally, in another embodiment, the matrix determination module comprises: The Jacobian matrix determination module is configured to calculate a Jacobian matrix of the kinematic cost function and a Jacobian matrix of the collision cost function in the target function. The summation module is configured to sum the Jacobian matrix of the kinematic cost function and the Jacobian matrix of the collision cost function as the Jacobian matrix of the target function.

[0117] Optionally, the embodiment of the present application further provides an electronic device, comprising: The electronic device comprises a processor, a memory, and a program or instruction stored in the memory and executable on the processor, and the program or instruction is executed by the processor to implement the steps of the vehicle parking trajectory smoothing optimization method.

[0118] Optionally, the embodiment of the present application further provides a vehicle comprising the electronic device.

[0119] Optionally, the embodiment of the present application further provides a readable storage medium, and the readable storage medium stores a program or instruction, and the program or instruction is executed by a processor to implement the steps of the vehicle parking trajectory smoothing optimization method.

[0120] Optionally, the embodiment of the present application further provides a computer program product, and the computer program product comprises a computer program or instruction, and the computer program or instruction is executed by a processor of an electronic device to implement the steps of the vehicle parking trajectory smoothing optimization method.

[0121] Regarding the device in the above-mentioned embodiments, the specific manners in which various modules perform operations have been described in detail in the embodiments of the method, and will not be described in detail here.

[0122] The apparatus embodiments described above are merely illustrative, wherein the units described as separate components can or can not be physically separate, and the components displayed as units can or can not be physical units, i.e., can be located in one place, or can be distributed on multiple network units. Part or all of the modules can be selected to achieve the purposes of the embodiments according to actual needs. Those skilled in the art can understand and implement without creative labor.

[0123] Figure 6 is a block diagram of an electronic device 600 provided by an embodiment of the present application Figure 6 As shown, it includes a processor 601, a communication interface 602, a memory 603 and a communication bus 604, wherein the processor 601, the communication interface 602 and the memory 603 complete mutual communication through the communication bus 604; The memory 603 is configured to store processor-executable instructions. The processor 601 is configured to execute the processor-executable instructions on the memory 603 to implement the method as described above.

[0124] In this embodiment, the communication bus can be a peripheral component interconnect (PCI) bus or an extended industry standard architecture (EISA) bus, etc. The communication bus can be divided into an address bus, a data bus, a control bus, etc. For the convenience of representation, only one thick line is shown in the figure, but it does not mean that there is only one bus or only one type of bus.

[0125] The communication interface is configured to communicate between the electronic device and other devices.

[0126] The memory can include a random access memory (RAM) and can also include a non-volatile memory, such as at least one disk memory. Optionally, the memory can also be at least one storage device located away from the aforementioned processor.

[0127] The processor described above can be a general processor, including a central processing unit (CPU), a network processor (NP), etc.; can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field-programmable gate array (FPGA) or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component.

[0128] In another embodiment provided in the present application, a vehicle is also provided, including the electronic device as described above. Figure 6 The electronic device as described above.

[0129] In another embodiment provided in the present application, a readable storage medium is also provided, when instructions in the readable storage medium are executed by a processor of an electronic device, the electronic device can execute various processes of the vehicle parking trajectory smoothing optimization method embodiment as described above, and can achieve the same technical effects. To avoid repetition, it will not be repeated here. For example, the readable storage medium includes a computer readable storage medium, such as a ROM, a random access memory (RAM), a CD-ROM, a magnetic tape, a floppy disk, and an optical data storage device, etc.

[0130] In another embodiment provided in the present application, a computer program product is also provided, including a computer program or instructions, when the computer program or instructions are executed by a processor of an electronic device, various processes of the vehicle parking trajectory smoothing optimization method embodiment as described above are implemented, and the same technical effects can be achieved. To avoid repetition, it will not be repeated here.

[0131] In the embodiments described above, the entire or part of the embodiments can be implemented by using software, hardware, firmware or any combination thereof. When implemented by using software, the entire or part of the embodiments can be implemented in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded into and executed by a computer, the entire or part of the procedures or functions according to the embodiments of the present application are generated. The computer can be a general purpose computer, a special purpose computer, a computer network, or other programmable apparatuses. The computer instructions can be stored in a computer readable storage medium or transmitted from one computer readable storage medium to another computer readable storage medium, for example, the computer instructions can be transmitted from a website, a computer, a server or a data center to another website, computer, server or data center through a wired (for example, coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (for example, infrared, wireless, microwave, etc.) manner. The computer readable storage medium can be any available medium or a data storage device including one or more available media integrated into a server, data center, etc. The available medium can be a magnetic medium (for example, floppy disk, hard disk, magnetic tape), an optical medium (for example, DVD), or a semiconductor medium (for example, Solid State Disk (SSD)), etc.

[0132] Figure 7 is a block diagram of an apparatus 700 for vehicle parking trajectory smoothing optimization provided by an embodiment of the present application. For example, the apparatus 700 can be provided as a server. Referring to Figure 7 , the apparatus 700 includes a processing component 722, which further includes one or more processors, and a memory resource represented by a memory 732, for storing instructions, such as an application program, executable by the processing component 722. The application program stored in the memory 732 can include one or more than one module, each of which corresponds to a set of instructions. In addition, the processing component 722 is configured to execute the instructions to perform the method described above.

[0133] The apparatus 700 can also include a power supply component 726 configured to perform power management of the apparatus 700, a wired or wireless network interface 750 configured to connect the apparatus 700 to a network, and an input / output (I / O) interface 758. The apparatus 700 can operate based on an operating system stored in the memory 732, such as Windows ServerTM, Mac OS XTM, UnixTM, LinuxTM, FreeBSDTM or the like.

[0134] Other embodiments of the application will be apparent to those skilled in the art from consideration of the specification and practice of the application disclosed herein. It is intended that the specification and examples be considered as exemplary only, with the true scope and spirit of the application being indicated by the following claims.

[0135] It is to be understood that the application is not limited to the precise construction herein disclosed and shown in the drawings, and that various changes in shape, size and arrangements of parts can be made without departing from the scope of the application. The scope of the application is limited only by the claims that follow.

Claims

1. A method for smoothing and optimizing vehicle parking trajectory, characterized in that, include: The coordinate information of obstacles around the vehicle and the reference trajectory of the vehicle are obtained. The coordinate information is used to represent the position of the obstacles in a coordinate system constructed with the vehicle as the reference. Based on the coordinate information, determine the map value of the obstacle on the robot's central Euclidean symbolic distance field RC_ESDF map, where the map value represents the shortest distance from the obstacle corresponding to the coordinate information to the outline of the vehicle; Based on the map value of each obstacle, the gradient information of the collision cost is determined, and the gradient information is used as the obstacle constraint relationship for optimizing the trajectory. Based on the parking reference trajectory and the obstacle constraint relationship, an objective function is constructed; The objective function is optimized using the nonlinear optimization problem solver IPOPT, and the obtained optimal solution is used as the smoothed and optimized parking trajectory.

2. The vehicle parking trajectory smoothing optimization method according to claim 1, characterized in that, The process of determining the gradient information of the collision cost based on the map value of each obstacle, and using the gradient information as the obstacle constraint relationship for optimizing the trajectory, includes: Obtain the global pose of the vehicle in the coordinate system, the global pose including: the vehicle's position point and orientation; Based on the vehicle's position and orientation, and the map value of each obstacle, the gradient information of the collision cost of each obstacle with respect to the vehicle's global pose is calculated using a collision cost function; The gradient information of the collision cost is used as an obstacle constraint relationship for optimizing the trajectory, so as to guide the vehicle away from the obstacle in the trajectory optimization.

3. The vehicle parking trajectory smoothing optimization method according to claim 1, characterized in that, The objective function is constructed based on the parking reference trajectory and the obstacle constraint relationship, including: The state variables and control variables of the parking reference trajectory are determined. The state variables include the vehicle's coordinate position, speed, and heading angle. The control variables include the vehicle's front wheel steering angle and acceleration. Based on the state variables, control variables, and obstacle constraints of the reference trajectory, an objective function is constructed using the collision avoidance optimization (OBCA) method. The objective function includes the sum of the kinematic cost function and the collision cost function, as well as the constraint conditions.

4. The vehicle parking trajectory smoothing optimization method according to claim 1, characterized in that, The process of using the nonlinear optimization problem solver IPOPT to optimize the objective function and using the obtained optimal solution as the smoothed parking trajectory includes: Determine the Jacobian matrix of the objective function; The Jacobian matrix is ​​optimized using the nonlinear optimization problem solver IPOPT, and the resulting optimal solution is used as the smoothed and optimized parking trajectory.

5. The vehicle parking trajectory smoothing optimization method according to claim 4, characterized in that, Determining the Jacobian matrix of the objective function includes: Calculate the Jacobian matrix of the kinematic cost function and the Jacobian matrix of the collision cost function in the objective function; The Jacobian matrix of the kinematic cost function and the Jacobian matrix of the collision cost function are used as the Jacobian matrix of the objective function.

6. The vehicle parking trajectory smoothing optimization method according to claim 1, characterized in that, The step of determining the map value of each obstacle on the robot's central Euclidean symbolic distance field RC_ESDF map based on the coordinate information includes: Based on the coordinate information, the position of the obstacle is mapped to a preset grid on the RC_ESDF map, and the corresponding grid index is obtained; The map value for each obstacle is calculated based on the grid index.

7. The vehicle parking trajectory smoothing optimization method according to claim 6, characterized in that, The calculation of the map value for each obstacle based on the grid index includes: If the grid index is within the grid range of the RC_ESDF map and the obstacle is located at the center of the grid, then the distance value stored on the grid is directly read as the map value of the corresponding obstacle. If the grid index is within the grid range of the RC_ESDF map, and the obstacle is located in a non-center position within the grid, then the map value corresponding to the obstacle is calculated by bilinear interpolation, combined with the distance values ​​of all or part of the adjacent grids around the grid where the obstacle is located.

8. An electronic device, characterized in that, include: Including processor and memory; And a program or instructions stored on the memory and executable on the processor, wherein the program or instructions, when executed by the processor, implement the steps of the vehicle parking trajectory smoothing optimization method as described in any one of claims 1 to 7.

9. A vehicle, characterized in that, Includes the electronic device as described in claim 8.

10. A readable storage medium, characterized in that, The readable storage medium stores a program or instructions that, when executed by a processor, implement the steps of the vehicle parking trajectory smoothing optimization method as described in any one of claims 1 to 7.

Images