Path Planning Method and System for Autonomous Driving Vehicles in a Perceived Uncertain Environment

Abstract

The present application discloses a path planning method and system for an autonomous driving vehicle in a perception-uncertain environment, relating to the field of autonomous driving. The method includes establishing a general environment model based on the perception-uncertain environment in which the vehicle is located, and performing active perception control on the vehicle by using an extended Kalman filter for the relative states of the vehicle and obstacle feature points and a gradient-based active perception algorithm pre-constructed until the expected uncertainty of the vehicle with respect to the obstacle is less than a set value. The updated state, covariance matrix, and vehicle pose of the vehicle relative to each obstacle feature point at the current moment are obtained, and a sampling algorithm and a soft-constraint hybrid A* algorithm are used for path planning to obtain multiple paths. The path with the minimum cost function is calculated and selected to perform path tracking control on the vehicle, and a new path with the minimum cost function is obtained every set time to perform path tracking control on the vehicle until the vehicle reaches the vehicle target pose. The present application improves the safety of the vehicle driving path.

Description

Technical Field

[0001] This application relates to the field of autonomous driving, and particularly to path planning and systems for autonomous vehicles in a perceptually uncertain environment. Background Art

[0002] With the rapid development of today's social economy, in the field of autonomous driving, the research on path planning algorithms is of great significance. First of all, path planning algorithms can ensure the safe driving of vehicles. By calculating the safest and most feasible path from the starting point to the ending point, they help autonomous vehicles avoid collisions and other dangerous situations, while maximizing compliance with traffic rules and human behavior guidelines. Secondly, path planning algorithms can improve driving efficiency and reduce energy consumption. By calculating the shortest or fastest path from the starting point to the ending point, the driving distance and time are reduced; furthermore, by considering factors such as road gradients and traffic conditions, the most energy-efficient path is calculated to help reduce fuel consumption and improve the energy efficiency of the vehicle; finally, path planning algorithms can also consider passenger comfort. By planning a smooth and efficient path, sharp turns, sudden accelerations, and sudden decelerations are reduced, providing a better driving experience for the driver.

[0003] However, in actual driving, path planning is a challenging problem because path planning requires accurate perception information, but there are external factors interfering with precise perception. For example, strong light can make the images of vision sensors too bright to be used. In such an environment, the detection performance of perception algorithms may not be perfect, and there are errors and uncertainties in the perception system. Most existing research cannot achieve satisfactory planning effects in an uncertain environment with perception errors. Summary of the Invention

[0004] The purpose of this application is to provide a path planning method and system for autonomous vehicles in a perceptually uncertain environment, reduce the perception errors and uncertainties of the vehicle regarding obstacles in the environment, and improve the safety of the vehicle driving path.

[0005] To achieve the above purpose, this application provides the following solutions:

[0006] In a first aspect, this application provides a path planning method for an autonomous vehicle in a perceptually uncertain environment, including.

[0007] Establish a general environment model in a perceptually uncertain environment based on the perceptually uncertain environment in which the vehicle is located; the general environment model includes the initial pose of the vehicle, the positions of obstacles, and the target pose of the vehicle; the positions of the obstacles are represented by multiple feature points of the obstacles.

[0008] An extended Kalman filter for the relative state of pre - constructed vehicle and obstacle feature points is used to calculate the expected uncertainty of the vehicle with respect to the obstacle, and it is judged whether the expected uncertainty of the vehicle with respect to the obstacle is less than a set value. If not, a gradient - based active perception algorithm is used to perform active perception control on the vehicle according to the general environment model in the perception - uncertain environment until the expected uncertainty of the vehicle with respect to the obstacle is less than the set value; if so, the updated state of the vehicle with respect to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment are obtained.

[0009] Based on the updated state of the vehicle with respect to each obstacle feature point at the current moment and the covariance matrix at the current moment, a sampling algorithm is used to sample the obstacle multiple times to obtain various position distributions of the obstacle.

[0010] Based on the vehicle pose at the current moment, the vehicle target pose, and the various position distributions of the obstacle, a soft - constraint hybrid A* algorithm is used for path planning to obtain multiple paths.

[0011] The cost function of each path is calculated respectively, and the path with the minimum cost function among the multiple paths is selected to perform path tracking control on the vehicle. And every set time, the extended Kalman filter for the relative state of the vehicle and obstacle feature points is used to calculate the expected uncertainty of the vehicle with respect to the obstacle again to obtain a new path with the minimum cost function for path tracking control on the vehicle until the vehicle reaches the vehicle target pose.

[0012] In a second aspect, the present application provides an autonomous vehicle path planning system in a perception - uncertain environment, including.

[0013] A general environment establishment module for establishing a general environment model in the perception - uncertain environment based on the perception - uncertain environment where the vehicle is located; the general environment model includes the initial vehicle pose, the obstacle position, and the vehicle target pose; the obstacle position is represented by multiple feature points of the obstacle.

[0014] An active perception control module, connected to the general environment establishment module, for using an extended Kalman filter for the relative state of pre - constructed vehicle and obstacle feature points to calculate the expected uncertainty of the vehicle with respect to the obstacle, and judging whether the expected uncertainty of the vehicle with respect to the obstacle is less than a set value. If not, a gradient - based active perception algorithm is used to perform active perception control on the vehicle according to the general environment model in the perception - uncertain environment until the expected uncertainty of the vehicle with respect to the obstacle is less than the set value; if so, the updated state of the vehicle with respect to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment are obtained.

[0015] A sampling module, connected to the active perception control module, is configured to perform multiple samplings on obstacles based on the updated state of the vehicle relative to each obstacle feature point at the current moment and the covariance matrix at the current moment, so as to obtain various position distributions of the obstacles.

[0016] A path planning module, connected to the sampling module, the active perception control module, and the general environment establishment module respectively, is configured to perform path planning using a soft-constraint hybrid A* algorithm based on the vehicle pose at the current moment, the vehicle target pose, and the various position distributions of the obstacles, so as to obtain multiple paths.

[0017] A path tracking module, connected to the path planning module and the active perception control module respectively, is configured to calculate the cost function of each path respectively, select the path with the minimum cost function among the multiple paths to perform path tracking control on the vehicle, and at every set time interval, use the extended Kalman filter of the relative state of the vehicle and the obstacle feature points to calculate the expected uncertainty of the vehicle with respect to the obstacle again, so as to obtain a new path with the minimum cost function to perform path tracking control on the vehicle until the vehicle reaches the vehicle target pose.

[0018] According to the specific embodiments provided by the present application, the present application has the following technical effects:

[0019] The present application provides a method and system for path planning of an autonomous driving vehicle in a perception-uncertain environment. By using a gradient-based active perception algorithm, according to the general environment model, active perception control is performed on the vehicle in a perception-uncertain environment. When the expected uncertainty of the vehicle with respect to the obstacle is less than a set value, the state of the vehicle relative to the obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment are obtained, so that the vehicle can more accurately obtain the position of the obstacle, reduce the perception error and uncertainty of the vehicle with respect to the obstacle in the environment, and at every set time interval, the expected uncertainty of the vehicle with respect to the obstacle is recalculated to adjust the driving path of the vehicle in real time according to the current environment, so as to improve the safety of the vehicle driving path. Description of the Drawings

[0020] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required to be used in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0021] Figure 1 It is a schematic flowchart of the method for path planning of an autonomous driving vehicle in a perception-uncertain environment according to the present application.

[0022] Figure 2 Schematic diagram of a general environment model for sensing an uncertain environment.

[0023] Figure 3 For Figure 1 Schematic diagram of the refined process of step 102 in the path planning method for an autonomous vehicle in a sensing uncertain environment.

[0024] Figure 4 Schematic diagram of the collision probability calculation in the path planning method for an autonomous vehicle in a sensing uncertain environment provided by this application.

[0025] Reference numerals:

[0026] Initial vehicle pose - 1, vehicle - 2, binocular camera - 3, obstacle - 4, target vehicle pose - 5. Detailed implementation manners

[0027] Next, the technical solutions in the embodiments of this application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of this application. Obviously, the described embodiments are only a part of the embodiments of this application, rather than all the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of this application.

[0028] To make the purpose, features, and advantages of this application more obvious and understandable, the following further detailed description of this application will be given in conjunction with the accompanying drawings and specific implementation manners.

[0029] In an exemplary embodiment, as Figure 1 shown, a path planning method for an autonomous vehicle in a sensing uncertain environment is provided. This method is executed by a computer device, and specifically can be executed alone by a computer device such as a terminal or a server, or jointly executed by a terminal and a server. In the embodiments of this application, this method includes the following steps 101 to 105.

[0030] Step 101, establish a general environment model for a sensing uncertain environment based on the sensing uncertain environment where the vehicle is located; as Figure 2 shown, the general environment model includes the initial vehicle pose 1, the obstacle position, and the target vehicle pose 5; the obstacle position is represented by multiple feature points of the obstacle 4.

[0031] Step 102: Use the extended Kalman filter for the relative states of the vehicle and obstacle feature points that has been pre - constructed to calculate the expected uncertainty of the vehicle with respect to the obstacle, and determine whether the expected uncertainty of the vehicle with respect to the obstacle is less than the set value. If not, use the gradient - based active perception algorithm to perform active perception control on vehicle 2 according to the general environment model in the perception - uncertain environment until the expected uncertainty of the vehicle with respect to the obstacle is less than the set value. If so, obtain the updated state of the vehicle relative to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment.

[0032] Step 103: Based on the updated state of the vehicle relative to each obstacle feature point at the current moment and the covariance matrix at the current moment, use the sampling algorithm to sample the obstacle multiple times to obtain various position distributions of obstacle 4.

[0033] Step 104: Based on the vehicle pose at the current moment, the vehicle target pose 5, and the various position distributions of obstacle 4, use the soft - constraint hybrid A* algorithm for path planning to obtain multiple paths.

[0034] Step 105: Calculate the cost function of each path respectively, select the path with the minimum cost function among the multiple paths to perform path - tracking control on vehicle 2, and at every set time interval, use the extended Kalman filter for the relative states of the vehicle and obstacle feature points to calculate the expected uncertainty of the vehicle with respect to the obstacle again, so as to obtain a new path with the minimum cost function to perform path - tracking control on vehicle 2 until vehicle 2 reaches the vehicle target pose 5.

[0035] In another exemplary embodiment of the present application, as Figure 2 shown, the general environment model established in step 101 further includes a binocular camera 3.

[0036] After establishing the general environment model in the perception - uncertain environment based on the perception - uncertain environment where the vehicle is located, the method for path planning of an autonomous driving vehicle in the perception - uncertain environment further includes establishing a world coordinate system based on the general environment model, obtaining the state of the vehicle, the positions of obstacles, the position of the binocular camera, the initial pose 1 of the vehicle, and the vehicle target pose 5 in the world coordinate system; establishing a camera coordinate system based on the general environment model, and obtaining the positions of obstacles in the camera coordinate system.

[0037] In the world coordinate system, the vehicle state is , where is the x - axis coordinate of vehicle 2 in the world coordinate system, is the axis coordinate of vehicle 2 in the world coordinate system, is the z - axis coordinate of vehicle 2 in the world coordinate system, is the speed of vehicle 2 in the world coordinate system, is the heading angle of vehicle 2 in the world coordinate system, is the angular velocity of the heading angle of vehicle 2 in the world coordinate system.

[0038] The obstacle position is , and the obstacle position is represented by the feature points at intervals of 0.1 m on the edge of obstacle 4. Among them, S is the obstacle feature point, , is the x-axis coordinate of the S-th feature point of obstacle 4 in the world coordinate system, is the axis coordinate of the S-th feature point of obstacle 4 in the world coordinate system, is the z-axis coordinate of the S-th feature point of obstacle 4 in the world coordinate system.

[0039] The position of the binocular camera is , is the x-axis coordinate of binocular camera 3 in the world coordinate system, is the axis coordinate of binocular camera 3 in the world coordinate system, is the z-axis coordinate of binocular camera 3 in the world coordinate system, is the heading angle of binocular camera 3 in the world coordinate system. The binocular camera is set on the vehicle, and this application assumes that the position of the binocular camera is the same as the vehicle position.

[0040] The initial pose 1 of the vehicle is , is the x-axis coordinate of the initial pose 1 of the vehicle in the world coordinate system, is the axis coordinate of the initial pose 1 of the vehicle in the world coordinate system, is the z-axis coordinate of the initial pose 1 of the vehicle in the world coordinate system, is the heading angle of the initial pose 1 of the vehicle in the world coordinate system.

[0041] The target pose 5 of the vehicle is , is the x-axis coordinate of the target pose 5 of the vehicle in the world coordinate system, is the axis coordinate of the target pose 5 of the vehicle in the world coordinate system, is the z-axis coordinate of the target pose 5 of the vehicle in the world coordinate system, is the heading angle of the target pose 5 of the vehicle in the world coordinate system.

[0042] In the camera coordinate system, the obstacle position is , is the x-axis coordinate of the S-th feature point of obstacle 4 in the camera coordinate system, is the x-axis coordinate of the S-th feature point of obstacle 4 in the camera coordinate system, and is the z-axis coordinate of the S-th feature point of obstacle 4 in the camera coordinate system. Based on the vehicle state and obstacle position in the world coordinate system, the state of the vehicle relative to the obstacle feature points in the world coordinate system is obtained. Among them, the relative state of the vehicle relative to each obstacle feature point at time k is:

[0043] .

[0044] Among them, is the state of the vehicle relative to each obstacle feature point at time k in the world coordinate system, is the relative position of the vehicle relative to each obstacle feature point on the x-axis at time k in the world coordinate system, is the relative position of the vehicle relative to each obstacle feature point at time k in the world coordinate system on the axis, is the relative position of the vehicle relative to each obstacle feature point on the z-axis at time k in the world coordinate system, is the speed of the vehicle at time k in the world coordinate, is the heading angle of the vehicle at time k in the world coordinate system, is the angular velocity of the heading angle of the vehicle at time k in the world coordinate system. Among them, the relative position of the vehicle relative to each obstacle feature point on the x-axis and the relative position of the vehicle relative to each obstacle feature point on the axis are obtained by the binocular camera 3 located on the vehicle.

[0045] In step 102, the extended Kalman filter for the relative state of the vehicle and the obstacle feature points is specifically:

[0046] .

[0047] .

[0048] .

[0049] .

[0050] .

[0051] Among them, is the predicted state of the vehicle relative to each obstacle feature point at time k + 1, is the updated state of the vehicle relative to each obstacle feature point at time k, is the input control variable at time k, is the predicted covariance matrix at time k + 1, is the Jacobian matrix, is the state noise covariance matrix, is the covariance matrix at time k, is the Kalman gain at time k+1, is the Jacobian matrix of the measurement function, is the measurement noise covariance matrix, is the updated state of the vehicle relative to the obstacle feature point at time k+1, is the measurement value at time k+1, is the observation model at time k+1, is the covariance matrix at time k+1, is the identity matrix.

[0052] For time k+1, the relative state of the vehicle and the obstacle feature point is:

[0053] .

[0054] Among them, is the predicted state of the vehicle relative to each obstacle feature point at time k+1, is the updated state of the vehicle relative to each obstacle feature point at time k, is the input control variable at time k, is the speed of the vehicle at time k, is the heading angle of the vehicle at time k, is the angular velocity of the vehicle's heading angle at time k, is the time interval between time k+1 and time k.

[0055] When the angular velocity of the vehicle's heading angle at time k is 0, the relative state of the vehicle and the obstacle feature point at time k+1 is:

[0056] .

[0057] The Jacobian matrix is:

[0058] .

[0059] Among them, is the partial derivative matrix of the relative state of the vehicle and the obstacle feature point at time k+1 with respect to each state quantity.

[0060] At time k+1, the noise introduced in the relative state of the vehicle and the obstacle feature point mainly comes from two places: linear acceleration and heading angle acceleration noise. It is assumed that the linear acceleration and yaw angle acceleration satisfy a Gaussian distribution with a mean of 0 and variances of respectively. At this time, the noise brought is:

[0061] 。

[0062] Among them, G and are intermediate variables, is noise, is the linear acceleration noise value at time k, is the heading angular acceleration noise value at time k, and satisfy:

[0063] 。

[0064] Among them, is the linear acceleration variance, is the yaw angular acceleration variance.

[0065] Thus, the state noise covariance matrix Q is:

[0066] 。

[0067] The observation model , the Jacobian matrix of the measurement function and the measurement noise covariance matrix can be obtained according to the analysis of the binocular camera perception process. Taking the observation model , the Jacobian matrix of the measurement function and the measurement noise covariance matrix at time k as an example, the binocular observation model is:

[0068] 。

[0069] Among them, is the binocular observation model, b is the baseline distance of binocular camera 3, is the pixel abscissa of the obstacle feature point in the left camera at time k, is the pixel ordinate of the obstacle feature point in the left camera at time k, is the pixel abscissa of the obstacle feature point in the right camera at time k.

[0070] The abscissa and ordinate of the obstacle feature point in the pixel coordinate system of the left camera can be obtained through the camera imaging model. The left camera imaging model is:

[0071] 。

[0072] Among them, is the internal parameter matrix of the camera, is the rotation matrix of the left camera at time k, is the translation matrix of the left camera at time k, that is, the pose of the camera. is the z - axis coordinate of the S - th feature point of the obstacle 4 in the camera coordinate system, that is, the position of the obstacle in the camera coordinate system in . is the position of the obstacle in the world coordinate system . The imaging model principles of the right - eye camera and the left - eye camera are the same.

[0073] Jacobian matrix of the measurement function , then considering the noise during the imaging of the binocular camera 3, considering that the noise in the horizontal and vertical directions of the left - eye camera pixel coordinate system during imaging satisfies a mean of 0 and a variance of , then the imaging model of the left - eye camera becomes:[[]]

[0074] .

[0075] Among them, is the rotation matrix of the binocular camera 3 at time k, is the translation matrix of the binocular camera 3 at time k, that is, the pose of the camera. is the noise value in the horizontal direction of the left - eye camera pixel coordinate system at time k, is the noise value in the vertical direction of the left - eye camera pixel coordinate system at time k, satisfying:[[]]

[0076] .

[0077] The analysis process of the noise in the horizontal direction of the right - eye camera pixel coordinate system is the same as that of the left - eye camera.

[0078] Then the binocular observation model considering noise is:[[]]

[0079] .

[0080] In this application, the binocular camera 3 obtains the relative position between the left - eye camera and the obstacle feature points in the camera coordinate system. Assuming that the left - eye camera coincides with the center position of the vehicle's rear axle, and considering that the mean of the observation noise is 0, therefore, the observation model can ignore the influence of noise and can be simplified to:[[]]

[0081] .

[0082] Among them, is the rotation matrix between the camera coordinate system and the world coordinate system, is the predicted state of the vehicle relative to each obstacle feature point at time k, is the predicted relative position of the vehicle relative to each obstacle feature point in the x - axis at time k, is the vehicle relative to each obstacle feature point at time k in axis predicted relative position,[[]] The predicted relative position of the vehicle with respect to each obstacle feature point at time k on the z-axis.

[0083] Jacobian matrix of the measurement function is:

[0084] .

[0085] Among them, is the partial derivative matrix of the observation model with respect to each state quantity.

[0086] At this time, the influence of the observation noise on the observation result is the difference between the binocular observation model considering noise and the binocular observation model: .

[0087] Then the measurement noise covariance matrix : .

[0088] In another exemplary embodiment of the present application, the expected uncertainty of the vehicle with respect to the obstacle is expressed as the trace of the covariance matrix.

[0089] As Figure 3 shown, in step 102, an extended Kalman filter for the relative state of the vehicle and the obstacle feature points constructed in advance is used to calculate the expected uncertainty of the vehicle with respect to the obstacle, and it is judged whether the expected uncertainty of the vehicle with respect to the obstacle is less than a set value. If not, a gradient-based active perception algorithm is used to perform active perception control on vehicle 2 in the perception uncertain environment according to the general environment model until the expected uncertainty of the vehicle with respect to the obstacle is less than the set value; if so, the updated state of the vehicle with respect to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment are obtained, which specifically includes the following steps 201 to 203.

[0090] In step 201, for the motion control process at time k, an extended Kalman filter for the relative state of the vehicle and the obstacle feature points constructed in advance is used to obtain the updated state of the vehicle with respect to each obstacle feature point at time k+1 and the covariance matrix at time k+1 according to the input control variable at time k.

[0091] In step 202, the expected uncertainty of the vehicle with respect to the obstacle at time k+1 is calculated according to the covariance matrix at time k+1.

[0092] In step 203, it is judged whether the expected uncertainty of the vehicle with respect to the obstacle at time k+1 is less than the set value. If not, according to the updated state of the vehicle with respect to each obstacle feature point at time k+1 and the covariance matrix at time k+1, a gradient-based active perception algorithm is used to obtain the predicted state of the vehicle with respect to each obstacle feature point at time k+2.

[0093] According to the predicted state of the vehicle relative to each obstacle feature point at the k+2 moment, the Stanley path tracking algorithm is used to determine the input control variable at the k+1 moment, and the vehicle 2 is controlled to move in the perception uncertain environment, and the motion control process at the k+1 moment is carried out until the expected uncertainty of the vehicle 2 with respect to the obstacle is less than the set value.

[0094] If so, obtain the updated state of the vehicle relative to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment.

[0095] In another exemplary embodiment of the present application, the active perception problem is modeled by a recursive Bayesian filter:

[0096] 。

[0097] Among them, is the state estimation distribution at the k moment, that is, the state distribution of the vehicle relative to each obstacle feature point in the world coordinate system at the k moment, is the state distribution of the vehicle relative to each obstacle feature point in the world coordinate system at the k-1 moment, is the state of the vehicle relative to each obstacle feature point at the k moment, is the state of the vehicle relative to each obstacle feature point at the k-1 moment, is the observed quantity at the k moment, is the input control variable at the k moment, is the normalization factor at the k moment. After being modeled by the recursive Bayesian filter, the active perception problem can be expressed as: seeking a control strategy to minimize the expected uncertainty of the state estimation. The uncertainty can be defined as the cost function to be optimized, and the process of minimizing the cost function is the process of minimizing the uncertainty. Select the trace of the covariance matrix as the cost function, that is: 。The active perception problem can be described in the following form:

[0098] 。

[0099] Active perception is an online solution process, which needs to pay attention to the solution efficiency of the equation. The gradient descent method is used to make the cost function decrease rapidly along the gradient. That is: Step 203, the specific calculation formula of the gradient-based active perception algorithm is:

[0100] 。

[0101] Among them, is the predicted state of the vehicle relative to each obstacle feature point at the k+2 moment, is the updated state of the vehicle relative to each obstacle feature point at time k+1, is the covariance matrix at time k+1, is the trace of the covariance matrix at time k+1, represents the expected value, is the gradient.

[0102] In another exemplary embodiment of the present application, in step 201, the input control variable is the front wheel steering angle of the vehicle.

[0103] Using the pre-constructed extended Kalman filter for the relative state of the vehicle and obstacle feature points, based on the input control variable at time k, obtain the updated state of the vehicle relative to each obstacle feature point at time k+1 and the covariance matrix at time k+1, specifically including the following steps 301 to 303.

[0104] Step 301, determine the yaw rate of the vehicle at time k according to the front wheel steering angle of the vehicle at time k, the yaw angle of the vehicle at time k, and the wheelbase of the vehicle's front and rear wheels.

[0105] Step 302, determine the predicted state of the vehicle relative to each obstacle feature point at time k+1 according to the yaw rate of the vehicle at time k.

[0106] Step 303, using the pre-constructed extended Kalman filter for the relative state of the vehicle and obstacle feature points, determine the updated state of the vehicle relative to each obstacle feature point at time k+1 and the covariance matrix at time k+1 according to the predicted state of the vehicle relative to each obstacle feature point at time k+1.

[0107] In another exemplary embodiment of the present application, in step 301, the following formula is used to determine the yaw rate of the vehicle at time k:

[0108] .

[0109] Wherein, is the yaw rate of the vehicle at time k, is the input control variable at time k, is the yaw angle of the vehicle at time k, is the wheelbase of the vehicle's front and rear wheels.

[0110] In another exemplary embodiment of the present application, after the active perception vehicle control in step 102, obtain the updated state of the vehicle relative to each obstacle feature point and the covariance matrix at the current moment. The updated state of the vehicle relative to each obstacle feature point and the covariance matrix at the current moment satisfy the normal distribution . The path planning algorithm is planned through a two-dimensional grid map without considering the state quantity of the direction. Therefore, the Box-Muller sampling algorithm is used for the first two state quantities in the updated state of the vehicle relative to each obstacle feature point at the current moment to perform multiple samplings, that is, multiple samplings are performed on the relative position relationship between the vehicle and each obstacle feature point in the world coordinate system to obtain various environmental state distributions.

[0111] Step 103: Based on the updated state of the vehicle relative to each obstacle feature point at the current moment and the covariance matrix at the current moment, use the sampling algorithm to perform multiple samplings on the obstacles to obtain various position distributions of the obstacles. The specific calculation formula is:

[0112] .

[0113] .

[0114] .

[0115] .

[0116] Among them, and are two random numbers that follow a uniform distribution on [0, 1] for each sampling, and are sampling data and follow the standard normal distribution N(0, 1). The current moment is the t moment, is the relative position of the vehicle relative to each obstacle feature point on the x-axis in the world coordinate system obtained by the mth sampling at the t moment, is the relative position of the vehicle relative to each obstacle feature point in the world coordinate system obtained by the mth sampling at the t moment in axis, , is the number of samplings, is the relative position of the vehicle relative to each obstacle feature point on the x-axis at the t moment in the world coordinate system, is the relative position of the vehicle relative to each obstacle feature point in axis at the t moment in the world coordinate system, is the covariance matrix at the t moment.

[0117] In another exemplary embodiment of the present application, the pose of the vehicle at the current moment is set as the current node. Step 104: Based on the pose of the vehicle at the current moment, the target pose 5 of the vehicle, and the various position distributions of the obstacle 4, use the soft constraint hybrid A* algorithm to perform path planning to obtain multiple paths, which specifically include the following steps 401 to 403.

[0118] Step 401: Generate multiple cost maps based on various position distributions of the obstacle 4, the vehicle pose at the current moment, and the vehicle target pose 5.

[0119] Step 402: For the i-th iteration of each cost map, in the cost map, starting from the current node, the vehicle conducts a search once with three steering wheel angles and two motion directions of forward and backward, and expands multiple new nodes.

[0120] Step 403: Calculate the comprehensive priority of each new node, select the node with the minimum comprehensive priority among the multiple new nodes as the next node, and after setting the next node as the current node and conducting a search for a set number of times, conduct an RS parsing curve expansion search from the current node to the vehicle target pose 5, and determine whether there is a collision between the RS parsing curve from the current node to the vehicle target pose 5 and the obstacle 4. If not, the path planning is completed, and a path in the cost map is obtained; if there is a collision, conduct the (i + 1)-th iteration; the paths in the multiple cost maps form multiple paths.

[0121] Among them, in Step 401, generating multiple cost maps based on various position distributions of the obstacle, the vehicle pose at the current moment, and the vehicle target pose 5 specifically includes: initializing multiple grid maps based on various position distributions of the obstacle 4, the vehicle pose at the current moment, and the vehicle target pose 5; generating multiple cost maps based on the multiple grid maps.

[0122] To initialize each grid map, it is necessary to determine the size of the map, including the height and width. The upper bound of the x-axis in the grid map takes the maximum value of the abscissas of all obstacle positions, the vehicle pose at the current moment, and the vehicle target pose 5 plus 5 meters, and the lower bound of the x-axis in the grid map is the minimum value of the abscissas minus 5 meters; in the grid map the upper bound of the y-axis takes the maximum value of the ordinates of all obstacle positions, the vehicle pose at the current moment, and the vehicle target pose 5 plus 5 meters, and the lower bound of the y-axis is the minimum value of the ordinates minus 5 meters. Such setting of the map size allows the vehicle to bypass all obstacles 4 and travel along the position close to the boundary when the obstacles in the map are relatively dense. The resolution of the grid is selected as 0.5 meters, and the resolution affects the calculation accuracy and complexity. Create a grid according to the map size and resolution. Mark the obstacle positions in the grid. Set the grid cells to the obstacle state according to the size and shape of the obstacle 4 (for example, circular, rectangular).

[0123] Generate multiple cost maps based on multiple grid maps. Specifically, the cost map is used to represent the cost from the end point to each grid cell, and these costs affect the path selection. Starting from the current node, search along the grid map to create a heuristic cost map. Each node in the map contains the cost from the node to the target node, and the cost value is the accumulation of the grid distances moved from the node to the target node. The cost map serves as the heuristic function value for the hybrid A* algorithm without considering kinematic constraints but considering obstacle 4.

[0124] In another exemplary embodiment of the present application, in step 403, the specific calculation formula for the comprehensive priority of a node is:

[0125] 。

[0126] 。

[0127] Where is the comprehensive priority of node , is the cost function of node , is the heuristic function of node , is the steering wheel angle of node , is the steering wheel angle of node , is the vehicle driving direction of node , is the vehicle driving direction of node , is the steering wheel angle penalty gain, is the steering wheel angle change penalty gain, is the reverse driving penalty gain, is the gear shift penalty gain, is the collision probability soft constraint, , is the soft constraint gain, is the collision probability.

[0128] As Figure 4 shown, the specific calculation process of the collision probability is: Simplify the vehicle into a quadrilateral. Use a formula in the form of to represent the four sides of the quadrilateral, where is the unit normal vector of the r-th side line of the quadrilateral, is the r-th side of the quadrilateral, and r takes values of 1, 2, 3, and 4. For any obstacle feature point e, the position of this feature point is uncertain and satisfies a mean of and a variance of For the normal distribution, the mean distance from the feature point to any straight line is: , and the variance is: , the distribution probability that a single feature point is inside the straight line:

[0129] .

[0130] Among them, is the distribution probability that a single feature point is inside the straight line, is the error function, then the probability that this point collides with the vehicle is: .

[0131] This application proposes a soft-constrained hybrid A-star binocular vision autonomous driving vehicle path planning method based on an active perception algorithm and a sampling algorithm, which conducts path planning in an environment with perception uncertainty. First, a general environment model for path planning under perception uncertainty is established. Secondly, an active perception algorithm for autonomous driving vehicles based on binocular vision is proposed to control the vehicle movement and reduce the perception error and uncertainty of obstacles in the environment. Furthermore, sampling is performed in the environment with reduced perception uncertainty, and then the soft-constrained hybrid A-star algorithm is used for planning to generate multiple possible path schemes to cope with the perception uncertainty environment, and the optimal path is selected in real time according to the perception information, improving the safety of the vehicle driving path.

[0132] Based on the same inventive concept, the embodiment of this application also provides an autonomous driving vehicle path planning system under a perception uncertainty environment. The autonomous driving vehicle path planning system under the perception uncertainty environment specifically includes the following modules.

[0133] General environment establishment module, used to establish a general environment model under a perception uncertainty environment based on the perception uncertainty environment where the vehicle is located; the general environment model includes the initial pose 1 of the vehicle, the obstacle position, and the target pose 5 of the vehicle; the obstacle position is represented by multiple feature points of the obstacle 4.

[0134] Active perception control module, connected to the general environment establishment module, used to adopt an extended Kalman filter for the relative state of the vehicle and obstacle feature points pre-constructed to calculate the expected uncertainty of the vehicle with respect to the obstacle, and determine whether the expected uncertainty of the vehicle with respect to the obstacle is less than a set value. If not, then adopt an active perception algorithm based on the gradient, and according to the general environment model, perform active perception control on the vehicle 2 in the perception uncertainty environment until the expected uncertainty of the vehicle with respect to the obstacle is less than the set value; if so, obtain the updated state of the vehicle with respect to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment.

[0135] The sampling module, connected to the active perception control module, is configured to perform multiple samplings on the obstacle based on the updated state of the vehicle relative to each obstacle feature point at the current moment and the covariance matrix at the current moment, so as to obtain various position distributions of the obstacle.

[0136] The path planning module, connected to the sampling module, the active perception control module and the general environment establishment module respectively, is configured to perform path planning by using the soft-constraint hybrid A* algorithm based on the vehicle pose at the current moment, the vehicle target pose 5 and the various position distributions of the obstacle 4, so as to obtain multiple paths.

[0137] The path tracking module, connected to the path planning module and the active perception control module respectively, is configured to calculate the cost function of each path respectively, select the path with the minimum cost function among the multiple paths to perform path tracking control on the vehicle 2, and at every set time interval, use the extended Kalman filter of the relative state of the vehicle and the obstacle feature points to calculate the expected uncertainty of the vehicle with respect to the obstacle again, so as to obtain a new path with the minimum cost function to perform path tracking control on the vehicle 2 until the vehicle 2 reaches the vehicle target pose 5.

[0138] The technical features of the above embodiments can be combined arbitrarily. For the sake of concise description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope described in this specification.

[0139] In this article, specific examples are used to elaborate on the principle and implementation manner of the present application. The description of the above embodiments is only used to help understand the method and its core idea of the present application; at the same time, for those of ordinary skill in the art, according to the idea of the present application, there will be changes in the specific implementation manner and application scope. In summary, the content of this specification should not be construed as a limitation to the present application.

Claims

1. A path planning method for an autonomous driving vehicle in a perceived uncertain environment, characterized in that, The path planning method for an autonomous vehicle in a perception-uncertain environment includes: Establishing a general environment model in a perception-uncertain environment based on the perception-uncertain environment where the vehicle is located; the general environment model includes the initial pose of the vehicle, the positions of obstacles, and the target pose of the vehicle; the positions of the obstacles are represented by multiple feature points of the obstacles; Using an extended Kalman filter for the relative states of the vehicle and the feature points of the obstacles pre-constructed to calculate the expected uncertainty of the vehicle with respect to the obstacles, and determining whether the expected uncertainty of the vehicle with respect to the obstacles is less than a set value. If not, then using a gradient-based active perception algorithm, according to the general environment model, performing active perception control on the vehicle in the perception-uncertain environment until the expected uncertainty of the vehicle with respect to the obstacles is less than the set value. If so, then obtaining the updated state of the vehicle with respect to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment; Based on the updated state of the vehicle with respect to each obstacle feature point at the current moment and the covariance matrix at the current moment, using a sampling algorithm to sample the obstacles multiple times to obtain various position distributions of the obstacles; Based on the vehicle pose at the current moment, the target pose of the vehicle, and the various position distributions of the obstacles, using a soft-constrained hybrid A* algorithm for path planning to obtain multiple paths; Calculating the cost function of each path respectively, selecting the path with the minimum cost function among the multiple paths for path tracking control of the vehicle, and at every set time interval, using the extended Kalman filter for the relative states of the vehicle and the feature points of the obstacles to calculate the expected uncertainty of the vehicle with respect to the obstacles again to obtain a new path with the minimum cost function for path tracking control of the vehicle until the vehicle reaches the target pose of the vehicle.

2. The path planning method for an autonomous driving vehicle in a perceived uncertain environment according to claim 1, wherein, The relative state of the vehicle and the feature points of the obstacle is: ； Among them, is the predicted state of the vehicle relative to each obstacle feature point at time k + 1, is the updated state of the vehicle relative to each obstacle feature point at time k, is the input control variable at time k, is the speed of the vehicle at time k, is the heading angle of the vehicle at time k, is the angular velocity of the vehicle's heading angle at time k, is the time interval between time k + 1 and time k.

3. The path planning method for an autonomous driving vehicle in a perceptually uncertain environment according to claim 2, wherein The expected uncertainty of the vehicle with respect to the obstacle is expressed as the trace of the covariance matrix; Using an extended Kalman filter for the relative states of the vehicle and the feature points of the obstacles pre-constructed to calculate the expected uncertainty of the vehicle with respect to the obstacles, and determining whether the expected uncertainty of the vehicle with respect to the obstacles is less than a set value. If not, then using a gradient-based active perception algorithm, according to the general environment model, performing active perception control on the vehicle in the perception-uncertain environment until the expected uncertainty of the vehicle with respect to the obstacles is less than the set value. If so, then obtaining the updated state of the vehicle with respect to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment, specifically including: For the motion control process at the k-th moment, using the extended Kalman filter for the relative states of the vehicle and the feature points of the obstacles pre-constructed, and according to the input control variable at the k-th moment, obtaining the updated state of the vehicle with respect to each obstacle feature point at the (k + 1)-th moment and the covariance matrix at the (k + 1)-th moment; Calculating the expected uncertainty of the vehicle with respect to the obstacles at the (k + 1)-th moment according to the covariance matrix at the (k + 1)-th moment; Determine whether the expected uncertainty of the vehicle with respect to the obstacle at time k+1 is less than the set value. If not, then according to the updated state of the vehicle with respect to each obstacle feature point at time k+1 and the covariance matrix at time k+1, use the gradient-based active perception algorithm to obtain the predicted state of the vehicle with respect to each obstacle feature point at time k+2; According to the predicted state of the vehicle with respect to each obstacle feature point at time k+2, use the Stanley path tracking algorithm to determine the input control variable at time k+1, and control the vehicle to move in the perception-uncertain environment, and perform the motion control process at time k+1 until the expected uncertainty of the vehicle with respect to the obstacle is less than the set value; If so, obtain the updated state of the vehicle with respect to each obstacle feature point at the current time, the covariance matrix at the current time, and the vehicle pose at the current time.

4. The path planning method for an autonomous driving vehicle in a perceived uncertain environment according to claim 3, wherein The specific calculation formula of the gradient-based active perception algorithm: ； Among them, is the predicted state of the vehicle relative to each obstacle feature point at time k+2, is the updated state of the vehicle relative to each obstacle feature point at time k+1, is the covariance matrix at time k+1, is the trace of the covariance matrix at time k+1, represents the expected value, is the gradient, is the gradient of the updated state of the vehicle relative to each obstacle feature point at time k+1.

5. The path planning method for an autonomous driving vehicle in a perceptually uncertain environment according to claim 3, wherein The input control variable is the front wheel steering angle of the vehicle; Using the pre-constructed extended Kalman filter for the relative state of the vehicle and the obstacle feature points, according to the input control variable at time k, obtain the updated state of the vehicle with respect to each obstacle feature point at time k+1 and the covariance matrix at time k+1, specifically including: Determine the yaw rate of the vehicle at time k according to the front wheel steering angle of the vehicle at time k, the yaw angle of the vehicle at time k, and the wheelbase of the vehicle's front and rear wheels; Determine the predicted state of the vehicle with respect to each obstacle feature point at time k+1 according to the yaw rate of the vehicle at time k; Using the pre-constructed extended Kalman filter for the relative state of the vehicle and the obstacle feature points, determine the updated state of the vehicle with respect to each obstacle feature point at time k+1 and the covariance matrix at time k+1 according to the predicted state of the vehicle with respect to each obstacle feature point at time k+1.

6. The path planning method for an autonomous driving vehicle in a perceptually uncertain environment according to claim 5, wherein, Use the following formula to determine the yaw rate of the vehicle at time k: ； Among them, is the yaw rate of the vehicle at time k, is the input control variable at time k, is the yaw angle of the vehicle at time k, is the wheelbase of the vehicle's front and rear wheels.

7. The path planning method for an autonomous driving vehicle in a perceptually uncertain environment according to claim 1, wherein Based on the updated state of the vehicle with respect to each obstacle feature point at the current time and the covariance matrix at the current time, use the sampling algorithm to sample the obstacle multiple times to obtain various position distributions of the obstacle. The specific calculation formula is: ； ； ； ； Among them, and are two random numbers that follow a uniform distribution on [0, 1] for each sampling, and are sampling data and follow the standard normal distribution N(0, 1). The current time is the t-th moment, is the relative position of the vehicle with respect to each obstacle feature point on the x-axis in the world coordinate system at the m-th sampling at the t-th moment, is the relative position of the vehicle with respect to each obstacle feature point in the world coordinate system at the m-th sampling at the t-th moment in axis, , is the number of samplings, is the relative position of the vehicle with respect to each obstacle feature point on the x-axis at the t-th moment in the world coordinate system, is the relative position of the vehicle with respect to each obstacle feature point in the world coordinate system at the t-th moment in axis relative position, is the covariance matrix at the t-th moment.

8. The path planning method for an autonomous driving vehicle in a perceptually uncertain environment according to claim 1, wherein Set the vehicle pose at the current time as the current node; Based on the vehicle pose at the current time, the vehicle target pose, and the various position distributions of the obstacle, use the soft-constrained hybrid A* algorithm for path planning to obtain multiple paths, specifically including: Generate multiple cost maps based on the various position distributions of the obstacle, the vehicle pose at the current time, and the vehicle target pose; For the i-th iteration of each cost map, in the cost map, starting from the current node, the vehicle searches once with three steering wheel angles and two motion directions of forward and backward, and expands multiple new nodes; Calculate the comprehensive priority of each new node, select the node with the minimum comprehensive priority among multiple new nodes as the next node, and after setting the number of searches with the next node as the current node, perform an extended search of the RS analysis curve from the current node to the vehicle target pose, and determine whether there is a collision between the RS analysis curve from the current node to the vehicle target pose and the obstacle. If not, the path planning is completed, and a path in the cost map is obtained; if so, the (i + 1)-th iteration is performed; the paths in multiple cost maps form multiple paths.

9. The path planning method for an autonomous driving vehicle in a perceptually uncertain environment according to claim 8, wherein The specific calculation formula for the comprehensive priority of a node is: ； ； wherein, is the comprehensive priority of node ; is the cost function of node ; is the heuristic function of node ; is the steering wheel angle of node ; is the steering wheel angle of node ; is the vehicle driving direction of node ; is the vehicle driving direction of node ; is the steering wheel angle penalty gain; is the steering wheel angle change penalty gain; is the reverse driving penalty gain; is the gear shift penalty gain; is the soft constraint of collision probability; , is the soft constraint gain; is the collision probability.

10. An autonomous driving vehicle path planning system under a perceived uncertain environment, characterized in that, The path planning system for an autonomous driving vehicle in a perceptually uncertain environment includes: A general environment establishment module, configured to establish a general environment model in a perceptually uncertain environment based on the perceptually uncertain environment in which the vehicle is located; the general environment model includes the vehicle initial pose, the obstacle position, and the vehicle target pose; the obstacle position is represented by multiple feature points of the obstacle; An active perception control module, connected to the general environment establishment module, configured to use a pre-constructed extended Kalman filter for the relative state of the vehicle and the obstacle feature points to calculate the expected uncertainty of the vehicle with respect to the obstacle, and determine whether the expected uncertainty of the vehicle with respect to the obstacle is less than a set value. If not, use a gradient-based active perception algorithm to perform active perception control on the vehicle in the perceptually uncertain environment according to the general environment model until the expected uncertainty of the vehicle with respect to the obstacle is less than the set value; if so, obtain the updated state of the vehicle with respect to each obstacle feature point at the current moment, the covariance matrix at the current moment, and the vehicle pose at the current moment; A sampling module, connected to the active perception control module, configured to perform multiple samplings on the obstacle based on the updated state of the vehicle with respect to each obstacle feature point at the current moment and the covariance matrix at the current moment by using a sampling algorithm to obtain various position distributions of the obstacle; A path planning module, respectively connected to the sampling module, the active perception control module, and the general environment establishment module, configured to perform path planning by using a soft-constrained hybrid A* algorithm based on the vehicle pose at the current moment, the vehicle target pose, and the various position distributions of the obstacle to obtain multiple paths; A path tracking module, respectively connected to the path planning module and the active perception control module, configured to calculate the cost function of each path respectively, select the path with the minimum cost function among the multiple paths to perform path tracking control on the vehicle, and at every set time, use the extended Kalman filter for the relative state of the vehicle and the obstacle feature points to calculate the expected uncertainty of the vehicle with respect to the obstacle again to obtain a new path with the minimum cost function to perform path tracking control on the vehicle until the vehicle reaches the vehicle target pose.

Images