Multi-sensor information fusion intelligent automatic parking control device and method

Abstract

The invention relates to an intelligent automatic parking control device and method based on multi-sensor information fusion, and belongs to the field of vehicle intelligent control. The control device comprises an information acquisition module, a microcontroller and a vehicle control unit; the method comprises the following steps: acquiring parking environment information through the point cloud information acquisition sensor, and acquiring parking space image information through the image information acquisition sensor; acquiring the current speed of the vehicle through a vehicle speed signal acquisition sensor; the deflection amplitude of a front wheel rotation angle is collected through a front wheel rotation angle signal collection sensor; the preprocessed image information and the point cloud information are fused through an unscented Kalman filter, the front wheel turning angle is calculated according to the position coordinates of the current driving state of the vehicle and the vehicle course angle, and the vehicle speed and the front wheel turning angle are controlled through a vehicle control unit so that the vehicle can be parked along a planned path. According to the invention, the coverage scene of the automatic parking method is expanded, and the accuracy of parking space detection and the comfort and stability in the parking process are improved.

Description

Technical Field

[0001] The invention relates to an intelligent automatic parking control device and method for multi-sensor information fusion, belonging to the field of vehicle intelligent control. Background Art

[0002] Existing parking technologies have switched from relying on a single sensor to obtain parking space information to using a multi-sensor information fusion method to extract parking space information when performing parking space recognition, but the efficiency and accuracy of recognition still need to be improved. When using a geometric figure-based method for path planning, there is often a discontinuous curvature at the connection of the planned path, which will cause the vehicle to turn on the spot during the parking process. To solve this problem, many scholars have studied the parking path fitting curve, but the method is relatively complicated. When performing tracking control, the experience-based intelligent control method has high requirements for the design of the controller and requires the collection and processing of a large amount of experimental data, which is difficult to implement. Summary of the invention

[0003] The present invention provides an intelligent automatic parking control device and method based on multi-sensor information fusion, so as to expand the coverage scenarios of the automatic parking method, improve the accuracy of parking space detection, and the comfort and stability during parking.

[0004] The present invention adopts the following technical solution:

[0005] The intelligent automatic parking control device with multi-sensor information fusion described in the present invention includes an information acquisition module, a microcontroller and a vehicle controller;

[0006] The information acquisition module includes an image information acquisition sensor, a point cloud information acquisition sensor, a vehicle speed signal acquisition sensor, and a front wheel angle signal acquisition sensor;

[0007] The information acquisition module transmits the collected vehicle information to the microcontroller, and the microcontroller controls the vehicle controller to control the vehicle after calculation.

[0008] The present invention adopts a control method of an intelligent automatic parking control device using multi-sensor information fusion, and the steps are as follows:

[0009] Step 1: Use cameras and lidar to collect environmental data:

[0010] The parking environment information is obtained through the point cloud information collection sensor, and the presence of parking space is determined based on the vehicle body parameters;

[0011] Collect parking space image information through an image information acquisition sensor;

[0012] The current speed of the vehicle is collected through a vehicle speed signal collection sensor;

[0013] The deflection amplitude of the front wheel steering angle is collected by the front wheel steering angle signal collection sensor;

[0014] Step 2: Processing the image information collected by the image information collection sensor, including image grayscale conversion, image denoising, filtering out interference lines, and feature extraction;

[0015] Step 3: Process the point cloud information collected by the point cloud information collection sensor, including data filtering and feature extraction, to obtain the position coordinates of the obstacle vehicle;

[0016] Step 4: Based on the image and coordinate data provided in steps 2 and 3, an unscented Kalman filter (UKF) is used to fuse the pre-processed image information and point cloud information to improve the accuracy and robustness of parking space recognition and positioning;

[0017] Step 5: According to the parking space coordinates, the coordinates of the center point of the rear axle of the vehicle, the steering angle of the front wheel of the vehicle, and the three constraints of vehicle posture constraint, obstacle avoidance constraint, and vehicle performance constraint, a parking path is planned based on the geometric method;

[0018] Step 6: Use the model predictive control algorithm to perform trajectory tracking control. Calculate the front wheel turning angle based on the position coordinates of the vehicle's current driving state and the vehicle's heading angle. Control the vehicle speed and front wheel turning angle through the vehicle controller to park the vehicle along the planned path.

[0019] The control method of the intelligent automatic parking control device using multi-sensor information fusion of the present invention, the specific steps of step 2 for processing the image information collected by the image information collection sensor are as follows:

[0020] (1) Use the weighted average method to convert the image into grayscale;

[0021] The weighted average method calculates the grayscale value by taking the weighted average of the pixel values ​​of the three channels RGB (red, green, and blue). Usually, the weights of the three channels are 0.299, 0.587, and 0.114 respectively; the expression is as follows:

[0022] Gray value = 0.299 × R + 0.587 × G + 0.114 × B (1)

[0023] (2) Use a Gaussian filter to remove noise from the image; in order to improve recognition accuracy and robustness, the interfering straight lines are filtered out;

[0024] Use Gaussian filter to remove noise in the image: The specific formula of Gaussian function is as follows:

[0025]

[0026] In the formula, x and y represent the horizontal and vertical coordinates of each pixel in the image, respectively, and σ represents the smoothing parameter. When σ is small, the image edge positioning accuracy is high, but the image smoothing effect is weak and the ability to suppress noise is poor; when σ is large, the Gaussian semi-sliding template will increase, causing serious deviation of the image edge position and increasing the amount of calculation;

[0027] (3) Use the RANSAC algorithm to fit the straight lines in the image and filter out the straight lines that do not conform to the preset model; the specific steps are:

[0028] 1) Random sampling: A certain number of data points are randomly selected from the dataset as candidate sets, which will be used to fit the model.

[0029] 2) Model Fitting: Use the selected data points to fit the model. In parking line recognition, select the straight line model to fit the parking line.

[0030] 3) Internal point test: For the fitted model, calculate the fitting error of all data points to the model. Use distance metric to measure the error. Here, the distance from the point to the fitted line is used to measure the error. The distance from the point to the line can be calculated using the point to line distance formula, that is, the distance from the point (x, y) to the line Ax+By+C=0 is:

[0031] Where A, B, and C are the parameters of the fitted straight line, and |Ax+By+C| is the vertical distance from the point (x, y) to the straight line;

[0032] Then according to the predefined threshold, if the fitting error from the point to the model is less than the preset threshold, it is considered as an inlier;

[0033] 4) Model evaluation: determine whether the fitted model meets expectations and evaluate the quality of the fit. Specifically, the degree of fit of the model is evaluated by the number of internal points.

[0034] 5) Iterative update: Repeat the above steps multiple times, and select the model with the most internal points as the optimal model each time. The algorithm can set the number of iterations and the convergence conditions, and stop the iteration when certain conditions are met;

[0035] 6) Final model: After the iteration is completed, the optimal model is selected as the final fitting result;

[0036] (4) Use the Canny edge detection algorithm to extract the parking space lines in the image. The specific steps are as follows:

[0037] 1) Use the Canny edge detection algorithm to perform edge detection on the grayscale converted and denoised image. Call the Canny edge detection function and set appropriate parameters. The specific calculation formula of the Canny algorithm is as follows:

[0038]

[0039] Where G represents the 2D Gaussian variance, G n represents the 2D Gaussian equation in direction n, and I represents the image function.

[0040] 2) Non-maximum suppression: Perform non-maximum suppression on the edge image obtained by the Canny algorithm to refine the edges.

[0041] 3) Edge connection: The suppressed edges are connected to fill the broken edge parts so that the edges become complete lines.

[0042] 4) Edge screening: Filter the detected edges according to specific parking space line features. For example, parking space lines are usually long straight lines, so a certain length and angle threshold can be set to filter out possible parking space lines.

[0043] 5) Output results: Mark the filtered parking space lines on the original image, or output them as an independent data structure for subsequent parking space recognition and detection.

[0044] The control method of the intelligent automatic parking control device using multi-sensor information fusion of the present invention, step three processes the point cloud information collected by the point cloud information collection sensor, including data filtering and feature extraction, to obtain the position coordinates of the obstacle vehicle, and the specific steps are:

[0045] (1) Use Gaussian filtering to filter the point cloud data collected by the lidar to remove outliers and noise. The specific formula of the Gaussian function is the same as that in step 2 above;

[0046] (2) Extract the shape and location information of obstacles in the point cloud, that is, feature extraction. The specific steps are as follows:

[0047] 1) Use DBSCAN (Density-Based Spatial Clustering of Applications with Noise) clustering algorithm to segment obstacles on the preprocessed lidar data;

[0048] 2) Vehicle profile extraction and analysis:

[0049] By further processing the clustered point cloud data, points representing the vehicle contour are extracted. The present invention uses geometric features to identify and extract the vehicle contour and determine the contour coordinates. Specifically, the vehicle contour is identified and the contour coordinates are determined based on the convex hull of the point cloud. The specific steps are:

[0050] Calculate the convex hull of the point cloud data to obtain a convex polygon representing the vehicle shape;

[0051] Whether the convex hull represents the vehicle contour can be determined based on its geometric features such as area and perimeter;

[0052] The shape and orientation of the convex hull are further analyzed to determine the position and orientation of the vehicle.

[0053] In the control method of the intelligent automatic parking control device using multi-sensor information fusion of the present invention, in step 4, based on the data provided in steps 2 and 3, an unscented Kalman filter (UKF) is used to fuse the pre-processed image information and point cloud information to improve the accuracy and robustness of parking space recognition and positioning. The specific steps are as follows:

[0054] (1) System modeling:

[0055] Model the vehicle's position and state. This can be achieved by defining state variables and state transition equations. State variables can include information such as the vehicle's position, speed, and direction. The state transition equation describes how the state changes over time. Specifically, the general state equation is:

[0056] X(t+1)=f(x(t),W(t))(4)

[0057] Where, for different times t, W(k) is the Gaussian white noise of the random variable X, and f is the nonlinear state equation function.

[0058] (2) Observation model:

[0059] Define observation variables and observation equations to describe how the preprocessed image information and point cloud information are associated with the vehicle state. Image information and point cloud information can provide observations about the position, direction, and parking space line of the obstacle vehicle. Specifically, the general observation equation is:

[0060] Z(t)=h(x(t),V(t))(5)

[0061] Where V(t) is the Gaussian white noise of the observation variable Z, and h is the observation equation of the nonlinear system.

[0062] (3) UKF initialization:

[0063] Initialize the UKF state estimate and covariance matrix. The initial state can be obtained based on prior information or sensor measurements. Prior information refers to:

[0064] a. Initial state of the system:

[0065] Prior information may include estimates of the initial state of the system, such as initial position, speed, direction, etc. This information can be based on previous observations, historical data, or inferences about the system's behavior.

[0066] b. Dynamic characteristics of the system:

[0067] Prior information may include knowledge of the system's dynamic characteristics, such as the system's motion model, acceleration, turning rate, etc. This information can help determine the state transition equations and dynamic model.

[0068] c. Environmental conditions and constraints:

[0069] The prior information may also include information about the environmental conditions and constraints that the system is operating in. For example, for a self-parking vehicle, the prior information may include parking space size restrictions.

[0070] (4) Prediction step:

[0071] a. Status prediction:

[0072] According to the state transfer equation, the UKF is used to predict the state and estimate the vehicle state and state covariance at the next moment. A set of state vectors called sigma points are used to approximate the probability distribution of the system state. These sigma points are generated according to the system state and covariance matrix and are distributed along the main directions of the mean and variance of the state. Each sigma point performs state prediction according to the state transfer equation of the system to estimate the next state of the system.

[0073] b. Covariance prediction:

[0074] After state prediction, it is necessary to estimate the covariance matrix of the system state, that is, the uncertainty of the system state.

[0075] The predicted state estimates and related parameters are used to update the covariance matrix to reflect the prediction error and uncertainty.

[0076] (5) Update steps:

[0077] a. Measurement prediction

[0078] First, the predicted states are converted into predicted observations using the observation model of the system. The observation model describes the relationship between states and observations.

[0079] Estimate predicted observations of the system based on the predicted states and the observation model.

[0080] b. Calculate Kalman gain

[0081] Kalman gain is an important parameter for weighing the predicted state and the actual observation, which can be used to adjust the weight between the predicted value and the observed value. It is used to measure the difference between the predicted state and the actual observation.

[0082] Specifically, the calculation formula of Kalman gain is:

[0083]

[0084] Where P is the covariance matrix of the system.

[0085] c. State update: Use the measurement predictions, Kalman gain, and actual observations to update the system’s state estimate.

[0086] According to the weight of the Kalman gain, the system state is corrected and updated based on the difference between the predicted state and the actual observation.

[0087] Specifically, the formula for updating the covariance matrix of the state of the nonlinear system is:

[0088]

[0089] (6) Fusion: A weighted average strategy is used to fuse image information and point cloud information to obtain a more comprehensive and accurate vehicle state estimation.

[0090] (7) Repeated Iteration: Iterate the prediction and update steps to continuously track the vehicle status and integrate new observation information.

[0091] In the control method of the intelligent automatic parking control device using multi-sensor information fusion of the present invention, the specific steps of planning the parking path based on the geometric figure method in step 5 are:

[0092] (1) Establishing the parking path curve expression

[0093] The parking path planning of the present invention is based on a polynomial curve. In order to take into account the requirements of smooth parking curve and low calculation difficulty, a quintic polynomial curve is used. The corner point at the right rear of the parking space is taken as the origin of the coordinate system 0, and the expression of the quintic polynomial curve is set as:

[0094] y=k0+k1x+k2x 2 +k3x 3 +k4x 4 +k5x 5 (9)

[0095] In formula (9), k0~k5 are coefficients of the curve expression, x is the horizontal coordinate of the center point of the rear axle of the vehicle, and y is the vertical coordinate of the center point of the rear axle of the vehicle.

[0096] (2) Solving the coefficients of the curve expression

[0097] Solving the six coefficients requires six equations. By constraining the posture of the starting and ending positions, two-point restrictions and four equations can be obtained.

[0098] The vehicle body should be parallel to the lane line at the initial parking position, and the vehicle body should be parked horizontally in the parking space at the end position, that is, the heading angle of the vehicle at the initial parking position and the end parking position should be zero, and we get:

[0099]

[0100] When the vehicle is in the initial parking position, it has not started to turn, the steering wheel angle is 0, the path curvature is 0, and the curvature calculation formula is:

[0101]

[0102] In the formula, are the first and second order derivatives of y, respectively.

[0103] According to the above initial position path curvature is zero, the above formula (10) can be derived again to obtain the second-order derivative of the initial parking position is also zero, that is:

[0104]

[0105] When the vehicle is at the parking end position, in order to avoid damage to the steering mechanism, the steering wheel should be in the return state, the steering wheel angle is zero, and the curvature of the path is also zero, that is, the second-order derivative of the quintic polynomial path curve at the parking end position is zero, which can be obtained:

[0106]

[0107] Substituting the coordinates of the starting and ending points of parking into equation (9), we can get two equations, and then combine equations (10)-(14) to obtain a parking path trajectory. In the same scenario, the ending point of parking is fixed, but the starting point has an optional area, which is determined according to the obstacle avoidance constraints of the vehicle and the performance constraints of the vehicle.

[0108] In terms of vehicle obstacle avoidance constraints:

[0109] Define the left front point of the vehicle as point A, the left rear point as point B, the right front point as point D, and the right rear point as point C;

[0110] Define the front left and right points of the parking space as boundary points a and d respectively; define the rear left and right points of the parking space as boundary points b and c respectively;

[0111] Avoid collision between the left front point A of the vehicle and the lane boundary;

[0112] Avoid collision between the right side of the vehicle, point CD, and the parking space boundary point a;

[0113] Avoid collision between the right front point D of the vehicle and the parking space boundary ad;

[0114] Avoid collision between the right rear point C of the vehicle and the parking space boundary cd;

[0115] In terms of vehicle performance constraints:

[0116] Since the steering performance of the vehicle is limited by the structure of its steering system, the curvature K of the planned path curve should not be too large, and should be less than or equal to the inverse of the vehicle's minimum turning radius. It can be obtained:

[0117]

[0118] Where Rmin is the minimum turning radius corresponding to the center of the rear axle, and K is the path curvature;

[0119] In the control method of the intelligent automatic parking control device using multi-sensor information fusion of the present invention, in step six, a model predictive control algorithm is used to perform trajectory tracking control, and the front wheel steering angle is calculated according to the position coordinates of the current driving state of the vehicle and the vehicle heading angle, and the vehicle speed and the front wheel steering angle are controlled by the vehicle controller to make the vehicle park along the planned path, wherein the steps of the model predictive control algorithm are:

[0120] (1) Establishment of kinetic model:

[0121] First, you need to create Figure 8 The vehicle dynamics model shown in FIG. 1 describes the vehicle's motion equations, including the evolution of state variables such as the vehicle's speed, equivalent front wheel steering angle, and heading angle. Specifically, the vehicle's kinematic equations are:

[0122]

[0123] Where (x, y) is the vehicle position coordinate; υ is the vehicle speed; φ is the equivalent front wheel turning angle; θ is the heading angle; LL f -L r is the vehicle wheelbase.

[0124] (2) Determine the control objectives:

[0125] Determine the target trajectory that the vehicle needs to track, that is, the path planned in step 5.

[0126] (3) Set the optimization problem:

[0127] The trajectory tracking problem is transformed into an optimization problem. At each moment, the goal of the optimization problem is to minimize the gap between the vehicle's current state and the target trajectory while satisfying the vehicle dynamics model and the constraints in step 5.

[0128] (4) Predicting future status:

[0129] The established dynamic model and current status information are used to predict the vehicle's status in the future.

[0130] (5) Optimize control input:

[0131] At each moment, the optimal control input, including the vehicle's front wheel angle and speed, is obtained by solving the optimization problem to minimize the prediction error.

[0132] (7) Implementation of control:

[0133] The optimized control inputs are applied to the vehicle, adjusting its speed and direction so that it follows the planned path.

[0134] (8) Repeated iteration:

[0135] The above steps are repeated continuously, updating the state information, resolving the optimization problem, and implementing new control inputs at each moment to achieve continuous trajectory tracking control.

[0136] Beneficial Effects

[0137] The present invention provides an intelligent automatic parking control device and method with multi-sensor information fusion, which not only expands the coverage scene of the automatic parking method but also improves the accuracy of parking space detection: a multi-sensor information fusion method is used to extract parking space information, which is applicable to parking scenes with parking space lines as well as parking scenes formed by adjacent obstacles; a Gaussian filter is used to filter the point cloud data collected by the laser radar to remove outliers and noise, effectively improve the anti-noise ability, and ensure the stability of vehicle contour extraction; an unscented Kalman filter is used to fuse the pre-processed image information and point cloud information, thereby improving the accuracy of parking space detection and the efficiency of parking space recognition. BRIEF DESCRIPTION OF THE DRAWINGS

[0138] Figure 1 It is a schematic diagram of the composition of the intelligent automatic parking control device with multi-sensor information fusion of the present invention;

[0139] Figure 2 It is a schematic diagram of the working process of the intelligent automatic parking control device with multi-sensor information fusion of the present invention;

[0140] Figure 3 Level parking condition;

[0141] Figure 4 Schematic diagram of the collision between the left front point A of the vehicle and the lane boundary;

[0142] Figure 5 Schematic diagram of the collision between the right CD side of the vehicle and the parking space boundary point a;

[0143] Figure 6Schematic diagram of the collision between the right front point D of the vehicle and the parking space boundary ad;

[0144] Figure 7 Schematic diagram of the collision between the right rear point C of the vehicle and the parking space boundary cd;

[0145] Figure 8 Vehicle dynamics model. DETAILED DESCRIPTION

[0146] In order to make the purpose and technical solution of the embodiment of the present invention clearer, the technical solution of the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings of the embodiment of the present invention. Obviously, the described embodiment is a part of the embodiment of the present invention, not all of the embodiments. Based on the described embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0147] The intelligent automatic parking control device of the present invention with multi-sensor information fusion is as follows: Figure 1 As shown, it includes an information acquisition module, a microcontroller and a vehicle controller; the information acquisition module includes an image information acquisition sensor, a point cloud information acquisition sensor, a vehicle speed signal acquisition sensor, and a front wheel angle signal acquisition sensor; the information acquisition module transmits the collected vehicle information to the microcontroller, and the microcontroller controls the vehicle controller to control the vehicle after calculation.

[0148] like Figure 2 As shown: The control method of the intelligent automatic parking control device using multi-sensor information fusion, the steps are as follows:

[0149] 1. Use cameras and lidar to collect environmental data:

[0150] The parking environment information is obtained through the point cloud information collection sensor, and the presence of parking space is determined based on the vehicle body parameters;

[0151] Collect parking space image information through an image information acquisition sensor;

[0152] The current speed of the vehicle is collected through a vehicle speed signal collection sensor;

[0153] The deflection amplitude of the front wheel steering angle is collected by the front wheel steering angle signal collection sensor;

[0154] 2. Process the image information collected by the image information acquisition sensor, including image grayscale conversion, image denoising, filtering interference lines and feature extraction. Specifically: First, use the weighted average method to convert the image to grayscale; then use the Gaussian filter to remove the noise in the image; in order to improve the recognition accuracy and robustness, filter the interference lines, use the RANSAC algorithm to fit the lines in the image, and filter out the lines that do not conform to the preset model; finally, use the Canny edge detection algorithm to extract the parking space lines in the image.

[0155] (1) Weighted average method:

[0156] The weighted average method calculates the grayscale value by taking the weighted average of the pixel values ​​of the three channels RGB (red, green, and blue). Usually, the weights of the three channels are 0.299, 0.587, and 0.114, respectively. The algorithm is as follows:

[0157] Gray value = 0.299 × R + 0.587 × G + 0.114 × B (1)

[0158] (2) Use a Gaussian filter to remove noise from the image. The specific formula of the Gaussian function is as follows:

[0159]

[0160] In the formula, x and y represent the horizontal and vertical coordinates of each pixel in the image, respectively, and σ represents the smoothing parameter. When σ is small, the image edge positioning accuracy is high, but the image smoothing effect is weak and the ability to suppress noise is poor. When σ is large, the Gaussian semi-sliding template will increase, causing serious deviation of the image edge position and increasing the amount of calculation.

[0161] (3) The RANSAC algorithm selects a set of data from the data set to fit the model through random sampling, and calculates the fitting error based on the degree of fit between the predefined model and the data points. Through iteration, data points with fitting errors within a certain range are selected, and finally the parameters that meet the expected model are obtained. The specific steps are as follows:

[0162] 1) Random sampling: A certain number of data points are randomly selected from the dataset as candidate sets, which will be used to fit the model.

[0163] 2) Model Fitting: Use the selected data points to fit the model. In parking line recognition, select the straight line model to fit the parking line.

[0164] 3) Internal point test: For the fitted model, calculate the fitting error of all data points to the model. Use distance metric to measure the error. Here, the distance from the point to the fitted line is used to measure the error. The distance from the point to the line can be calculated using the point to line distance formula, that is, the distance from the point (x, y) to the line Ax+By+C=0 is: Where A, B, and C are the parameters of the fitted straight line, and |Ax+By+C| is the vertical distance from the point (x, y) to the straight line.

[0165] According to the predefined threshold, if the fitting error of the point to the model is less than the preset threshold, it is considered to be an inlier, that is, a data point that fits the model well enough.

[0166] 4) Model evaluation: Determine whether the fitted model meets expectations and evaluate the quality of the fit. Specifically, the number of internal points is used to evaluate the degree of fit of the model.

[0167] 5) Iterative update: Repeat the above steps multiple times, and select the model with the most inliers as the optimal model each time. The algorithm can set the number of iterations and the convergence conditions, and stop the iteration when certain conditions are met.

[0168] 6) Final model: After the iteration is completed, the optimal model is selected as the final fitting result.

[0169] (4) The specific steps of using the Canny edge detection algorithm to extract parking space lines in the image are as follows:

[0170] 1) Use the Canny edge detection algorithm to perform edge detection on the grayscale converted and denoised image. Call the Canny edge detection function and set appropriate parameters. The specific calculation formula of the Canny algorithm is as follows:

[0171]

[0172] Where G represents the 2D Gaussian variance, G n represents the 2D Gaussian equation in direction n, and I represents the image function.

[0173] 2) Non-maximum suppression: Perform non-maximum suppression on the edge image obtained by the Canny algorithm to refine the edges.

[0174] 3) Edge connection: The suppressed edges are connected to fill the broken edge parts so that the edges become complete lines.

[0175] 4) Edge screening: Filter the detected edges according to specific parking space line features. For example, parking space lines are usually long straight lines, so a certain length and angle threshold can be set to filter out possible parking space lines.

[0176] 5) Output results: Mark the filtered parking space lines on the original image, or output them as an independent data structure for subsequent parking space recognition and detection.

[0177] 3. Process the point cloud information collected by the point cloud information collection sensor, including data filtering and feature extraction, to obtain the position coordinates of the obstacle vehicle. Specifically:

[0178] (1) Use Gaussian filtering to filter the point cloud data collected by the lidar to remove outliers and noise. The specific formula of the Gaussian function is the same as that in step 2 above;

[0179] (2) Extract the shape and location information of obstacles in the point cloud, that is, feature extraction. The specific steps are as follows:

[0180] 1) Use DBSCAN (Density-Based Spatial Clustering of Applications with Noise) clustering algorithm to segment obstacles on the preprocessed lidar data;

[0181] The DBSCAN algorithm divides the LiDAR data points into multiple clusters. Each cluster represents an obstacle, which may contain a vehicle. The DBSCAN algorithm can identify clusters of points around the vehicle, which may form the outline of the vehicle. Among them, the parameters that the DBSCAN algorithm needs to determine include epsilon (neighborhood radius) and minPoints (minimum number of points). Epsilon determines the neighborhood range of a point, and minPoints determines the minimum number of neighbors of a core point.

[0182] 2) Vehicle profile extraction and analysis:

[0183] By further processing the clustered point cloud data, points representing the vehicle contour are extracted. The present invention uses geometric features to identify and extract the vehicle contour and determine the contour coordinates. Specifically, the vehicle contour is identified and the contour coordinates are determined based on the convex hull of the point cloud. The specific steps are:

[0184] The convex hull is calculated for the point cloud data to obtain a convex polygon representing the vehicle shape.

[0185] Whether the convex hull represents the vehicle contour can be determined based on its geometric features such as area and perimeter.

[0186] The shape and orientation of the convex hull are further analyzed to determine the position and orientation of the vehicle.

[0187] The general steps of the above convex hull algorithm are:

[0188] a. Determine the point set:

[0189] Determine the point set for which the convex hull needs to be calculated, usually expressed as P = {p1, p2, ..., pn}, where pi represents the i-th point in the point set.

[0190] b. Find the reference point:

[0191] Select a reference point from the point set P. Common choices are the bottom left point or the point with the smallest x coordinate.

[0192] c. Sorting:

[0193] Sort all points in the point set P by their polar angles with the reference point (the angle relative to the horizontal line of the reference point). This step can be done using polar coordinates.

[0194] d. Construct the convex hull:

[0195] Constructing a convex hull from a set of ordered points usually uses a stack data structure to assist in implementation.

[0196] Traverse the sorted point set in sequence, for each point:

[0197] If the straight line formed by the current point and the first two stack vertices (the points that have been determined on the convex hull) is a left turn or a point on a straight line, the point is pushed into the stack.

[0198] Otherwise, pop the stack vertices until a left-turning corner is formed or the stack is empty, and then push the current point onto the stack.

[0199] e. Output convex hull:

[0200] After the convex hull is constructed, the remaining points in the stack are the points on the convex hull. These points can be taken out of the stack to form a sequential list of the convex hull.

[0201] 4. Based on the data provided in steps 2 and 3, the unscented Kalman filter (UKF) is used to fuse the preprocessed image information and point cloud information to improve the accuracy and robustness of parking space recognition and positioning. The specific steps are as follows:

[0202] (1) System modeling:

[0203] Model the vehicle's position and state. This can be achieved by defining state variables and state transition equations. State variables can include information such as the vehicle's position, speed, and direction. The state transition equation describes how the state changes over time. Specifically, the general state equation is:

[0204] X(t+1)=f(x(t),W(t))(4)

[0205] Where, for different times t, W(k) is the Gaussian white noise of the random variable X, and f is the nonlinear state equation function.

[0206] (2) Observation model:

[0207] Define observation variables and observation equations to describe how the preprocessed image information and point cloud information are associated with the vehicle state. Image information and point cloud information can provide observations about the position, direction, and parking space line of the obstacle vehicle. Specifically, the general observation equation is:

[0208] Z(t)=h(x(t),V(t))(5)

[0209] Where V(t) is the Gaussian white noise of the observation variable Z, and h is the observation equation of the nonlinear system.

[0210] (3) UKF initialization:

[0211] Initialize the state estimate and covariance matrix of the UKF. The initial state can be obtained based on prior information or sensor measurements.

[0212] Among them, prior information refers to:

[0213] a. Initial state of the system:

[0214] Prior information may include estimates of the initial state of the system, such as initial position, speed, direction, etc. This information can be based on previous observations, historical data, or inferences about the system's behavior.

[0215] b. Dynamic characteristics of the system:

[0216] Prior information may include knowledge of the system's dynamic characteristics, such as the system's motion model, acceleration, turning rate, etc. This information can help determine the state transition equations and dynamic model.

[0217] c. Environmental conditions and constraints:

[0218] The prior information may also include information about the environmental conditions and constraints that the system is operating in. For example, for a self-parking vehicle, the prior information may include parking space size restrictions.

[0219] (4) Prediction step:

[0220] a. Status prediction:

[0221] According to the state transfer equation, the UKF is used to predict the state and estimate the vehicle state and state covariance at the next moment. A set of state vectors called sigma points are used to approximate the probability distribution of the system state. These sigma points are generated according to the system state and covariance matrix and are distributed along the main directions of the mean and variance of the state. Each sigma point performs state prediction according to the state transfer equation of the system to estimate the next state of the system.

[0222] b. Covariance prediction:

[0223] After state prediction, it is necessary to estimate the covariance matrix of the system state, that is, the uncertainty of the system state.

[0224] The predicted state estimates and related parameters are used to update the covariance matrix to reflect the prediction error and uncertainty.

[0225] (5) Update steps:

[0226] a. Measurement prediction

[0227] First, the predicted states are converted into predicted observations using the observation model of the system. The observation model describes the relationship between states and observations.

[0228] Estimate predicted observations of the system based on the predicted states and the observation model.

[0229] b. Calculate Kalman gain

[0230] Kalman gain is an important parameter for weighing the predicted state and the actual observation, which can be used to adjust the weight between the predicted value and the observed value. It is used to measure the difference between the predicted state and the actual observation.

[0231] Specifically, the calculation formula of Kalman gain is:

[0232]

[0233] Where P is the covariance matrix of the system.

[0234] c. State update: Use the measurement predictions, Kalman gain, and actual observations to update the system’s state estimate.

[0235] According to the weight of the Kalman gain, the system state is corrected and updated based on the difference between the predicted state and the actual observation.

[0236] Specifically, the formula for updating the covariance matrix of the state of the nonlinear system is:

[0237]

[0238]

[0239] (6) Fusion: A weighted average strategy is used to fuse image information and point cloud information to obtain a more comprehensive and accurate vehicle state estimation.

[0240] (7) Repeated Iteration: Iterate the prediction and update steps to continuously track the vehicle status and integrate new observation information.

[0241] 5. According to the parking space coordinates, the coordinates of the center point of the vehicle's rear axle, and the front wheel angle of the vehicle, combined with the three constraints of vehicle posture constraint, obstacle avoidance constraint, and vehicle performance constraint, the parking path is planned based on the geometric method. The specific steps are as follows:

[0242] (1) Establishing the parking path curve expression

[0243] The parking path planning of the present invention is based on a polynomial curve. In order to take into account the requirements of smooth parking curve and low calculation difficulty, a quintic polynomial curve is used. Figure 3 In the coordinate system, take the corner point c at the right rear of the parking space as the origin O of the coordinate system, and assume that the expression of the quintic polynomial curve is:

[0244] y=k0+k1x+k2x 2 +k3x 3 +k4x 4 +k5x 5 (9)

[0245] In formula (9), k0~k5 are coefficients of the curve expression, x is the horizontal coordinate of the center point of the rear axle of the vehicle, and y is the vertical coordinate of the center point of the rear axle of the vehicle.

[0246] (2) Solving the coefficients of the curve expression

[0247] Solving the six coefficients requires six equations. By constraining the posture of the starting and ending positions, two-point restrictions and four equations can be obtained.

[0248] The vehicle body should be parallel to the lane line at the initial parking position, and the vehicle body should be parked horizontally in the parking space at the end position, that is, the heading angle of the vehicle at the initial parking position and the end parking position should be zero, and we get:

[0249]

[0250] When the vehicle is in the initial parking position, it has not started to turn, the steering wheel angle is 0, the path curvature is 0, and the curvature calculation formula is:

[0251]

[0252] In the formula, are the first and second order derivatives of y, respectively.

[0253] According to the above initial position path curvature is zero, the above formula (10) can be derived again to obtain the second-order derivative of the initial parking position is also zero, that is:

[0254]

[0255] When the vehicle is at the parking end position, in order to avoid damage to the steering mechanism, the steering wheel should be in the return state, the steering wheel angle is zero, and the curvature of the path is also zero, that is, the second-order derivative of the quintic polynomial path curve at the parking end position is zero, which can be obtained:

[0256]

[0257] Substituting the coordinates of the starting and ending points of parking into equation (9), we can get two equations, and then combine equations (10)-(14) to obtain a parking path trajectory. In the same scenario, the ending point of parking is fixed, but the starting point has an optional area, which is determined according to the obstacle avoidance constraints of the vehicle and the performance constraints of the vehicle.

[0258] In terms of vehicle obstacle avoidance constraints, four collision scenarios are considered:

[0259] First, as Figure 4 As shown, the vehicle body has not yet started to enter the parking space, and the left front point A of the vehicle collides with the lane boundary.

[0260] Second, if Figure 5 As shown, the vehicle body has not completely entered the parking space, and the right CD side of the vehicle collides with the parking space boundary point a.

[0261] Third, if Figure 6 As shown, when the vehicle body is straightened, the right front vertex D of the vehicle collides with the parking space boundary ad.

[0262] Fourth, if Figure 7 As shown, the right rear vertex C of the vehicle collides with the parking space boundary cd.

[0263] In order to avoid collisions, some constraints need to be set to leave a safe distance. The specific obstacle avoidance constraints are as follows:

[0264] like Figure 4 As shown in the figure, the vehicle body has not yet entered the parking space, that is, when the right side CD of the vehicle is outside the parking space boundary ab, the left front point A of the vehicle may collide with the lane boundary. In order to avoid collision, a certain safety distance L is left between the left front point A of the vehicle and the lane boundary. S1 , the constraints are:

[0265]

[0266] In formula (17), y A is the ordinate of the left front point A of the vehicle; Or is the ordinate of the center point of the rear axle of the vehicle; L is the length of the vehicle; L r is the vehicle rear overhang length; θ is the heading angle; W r is the width of the road; W p is the width of the parking space; x1 is the horizontal coordinate of the center point of the rear axle of the vehicle at the starting position of parking.

[0267] like Figure 5 As shown in the figure, when the vehicle body has not completely entered the parking space, that is, part of the right CD side of the vehicle is outside the parking space boundary ab side, the right CD side of the vehicle may collide with the parking space boundary point a. In order to avoid collision, the constraint conditions are:

[0268] y C +(L P -x C )tanθ≥W P ,x C ≤L P ∩x D ≥L P (18)

[0269] Figure 6 The figure shows the situation where the right front vertex D of the vehicle collides with the parking space boundary ad. In order to avoid collision, the right front vertex D of the vehicle should always be on the inner side of the parking space boundary ad and keep a certain safety distance L. S2 , the constraints are:

[0270]

[0271] Figure 7 The figure shows the situation where the right rear vertex C of the vehicle collides with the parking space boundary cd. In order to avoid collision, the right rear vertex C of the vehicle should always be on the inner side of the parking space boundary cd and keep a certain safety distance L. S3 , the constraints are:

[0272]

[0273] In terms of vehicle performance constraints:

[0274] Since the steering performance of the vehicle is limited by the structure of its steering system, the curvature K of the planned path curve should not be too large, and should be less than or equal to the inverse of the vehicle's minimum turning radius. It can be obtained:

[0275]

[0276] Where Rmin is the minimum turning radius corresponding to the center of the rear axle, and K is the path curvature;

[0277] 6. Use the model predictive control algorithm to perform trajectory tracking control, calculate the front wheel turning angle based on the position coordinates of the vehicle's current driving state and the vehicle's heading angle, and then control the vehicle speed and front wheel turning angle through the vehicle controller to park the vehicle along the planned path.

[0278] Specifically, the steps of the model predictive control algorithm are:

[0279] (1) Establishment of kinetic model:

[0280] First, you need to create Figure 8 The vehicle dynamics model shown in FIG. 1 describes the vehicle's motion equations, including the evolution of state variables such as the vehicle's speed, equivalent front wheel steering angle, and heading angle. Specifically, the vehicle's kinematic equations are:

[0281]

[0282] Where (x, y) is the vehicle position coordinate; υ is the vehicle speed; φ is the equivalent front wheel turning angle; θ is the heading angle; LL f -L r is the vehicle wheelbase.

[0283] The vehicle parameters are then determined, as shown in Table 1 below.

[0284] Table 1 Vehicle parameters

[0285]

[0286] (2) Determine the control objectives:

[0287] Determine the target trajectory that the vehicle needs to track, that is, the path planned in step 5.

[0288] (3) Set the optimization problem:

[0289] The trajectory tracking problem is transformed into an optimization problem. At each moment, the goal of the optimization problem is to minimize the gap between the vehicle's current state and the target trajectory while satisfying the vehicle dynamics model and the constraints in step 5.

[0290] (4) Predicting future status:

[0291] The established dynamic model and current status information are used to predict the vehicle's status in the future.

[0292] (5) Optimize control input:

[0293] At each moment, the optimal control input, including the vehicle's front wheel angle and speed, is obtained by solving the optimization problem to minimize the prediction error.

[0294] (7) Implementation of control:

[0295] The optimized control inputs are applied to the vehicle, adjusting its speed and direction so that it follows the planned path.

[0296] (8) Repeated iteration:

[0297] The above steps are repeated continuously, updating the state information, resolving the optimization problem, and implementing new control inputs at each moment to achieve continuous trajectory tracking control.

[0298] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. An intelligent automatic parking control device with multi-sensor information fusion, characterized in that: Including information acquisition module, microcontroller and vehicle controller; The information acquisition module includes an image information acquisition sensor, a point cloud information acquisition sensor, a vehicle speed signal acquisition sensor, and a front wheel angle signal acquisition sensor; The information acquisition module transmits the collected vehicle information to the microcontroller, and the microcontroller controls the vehicle controller to control the vehicle after calculation.

2. A control method for an intelligent automatic parking control device using multi-sensor information fusion, characterized in that: Here are the steps: Step 1: Use cameras and lidar to collect environmental data: The parking environment information is obtained through the point cloud information collection sensor, and the presence of parking space is determined based on the vehicle body parameters; Collect parking space image information through an image information acquisition sensor; The current speed of the vehicle is collected through a vehicle speed signal collection sensor; The deflection amplitude of the front wheel steering angle is collected by the front wheel steering angle signal collection sensor; Step 2: Processing the image information collected by the image information collection sensor, including image grayscale conversion, image denoising, filtering out interference lines, and feature extraction; Step 3: Process the point cloud information collected by the point cloud information collection sensor, including data filtering and feature extraction, to obtain the position coordinates of the obstacle vehicle; Step 4: Based on the image and coordinate data provided in steps 2 and 3, an unscented Kalman filter is used to fuse the pre-processed image information and point cloud information to improve the accuracy and robustness of parking space recognition and positioning; Step 5: According to the parking space coordinates, the coordinates of the center point of the rear axle of the vehicle, the steering angle of the front wheel of the vehicle, and the three constraints of vehicle posture constraint, obstacle avoidance constraint, and vehicle performance constraint, a parking path is planned based on the geometric method; Step 6: Use the model predictive control algorithm to perform trajectory tracking control. Calculate the front wheel turning angle based on the position coordinates of the vehicle's current driving state and the vehicle's heading angle. Control the vehicle speed and front wheel turning angle through the vehicle controller to park the vehicle along the planned path.

3. The control method of the intelligent automatic parking control device using multi-sensor information fusion according to claim 2 is characterized in that: Step 2: The specific steps of processing the image information collected by the image information collection sensor are as follows: (1) Use the weighted average method to convert the image into grayscale; The weighted average method calculates the grayscale value by taking the weighted average of the pixel values ​​of the three RGB channels; The weights of the red, green, and blue channels are 0.299, 0.587, and 0.114, respectively; their expressions are as follows: Gray value = 0.299 × R + 0.587 × G + 0.114 × B; (1) (2) Use a Gaussian filter to remove noise from the image; the specific formula of the Gaussian function is as follows: In the formula, x and y represent the horizontal and vertical coordinates of each pixel in the image, respectively, and σ represents the smoothing parameter. When σ is small, the image edge positioning accuracy is high, but the image smoothing effect is weak and the ability to suppress noise is poor; when σ is large, the Gaussian semi-sliding template will increase, causing serious deviation of the image edge position and increasing the amount of calculation; (3) Use the RANSAC algorithm to fit the straight lines in the image and filter out the straight lines that do not conform to the preset model; (4) Use the Canny edge detection algorithm to extract the parking space lines in the image.

4. The control method of the intelligent automatic parking control device using multi-sensor information fusion according to claim 3 is characterized in that: The specific steps of using the RANSAC algorithm to fit the straight lines in the image and filter out the straight lines that do not conform to the preset model are: (1) Random sampling: randomly select a certain number of data points from the data set as candidate sets, which will be used to fit the model; (2) Model fitting: Use the selected data points to fit the model; in parking line recognition, select a straight line model to fit the parking line; (3) Internal point test: For the fitted model, calculate the fitting error of all data points to the model; use distance metric to measure the error, and use the distance from the point to the fitted line to measure the error; The distance from a point to a line is calculated using the distance formula from a point to a line, that is, the distance from a point (x, y) to the line Ax+By+C=0 is: Where A, B, and C are the parameters of the fitted straight line, and |Ax+By+C| is the vertical distance from the point (x, y) to the straight line; Then according to the predefined threshold, if the fitting error from the point to the model is less than the preset threshold, it is considered as an inlier; (4) Model evaluation: determine whether the fitted model meets expectations and evaluate the quality of the fit; (5) Iterative update: Repeat the above steps multiple times, and each time select the model with the most inliers as the optimal model; (6) Final model: After the iteration is completed, the optimal model is selected as the final fitting result.

5. The control method of the intelligent automatic parking control device using multi-sensor information fusion according to claim 3 is characterized in that: The specific steps of using the Canny edge detection algorithm to extract parking space lines in an image are as follows: (1) Use the Canny edge detection algorithm to perform edge detection on the image after the grayscale conversion and denoising mentioned above; call the Canny edge detection function and set appropriate parameters. The specific calculation formula of the Canny algorithm is as follows: Where G represents the 2D Gaussian variance, G n represents the 2D Gaussian equation in direction n, and I represents the image function; (2) Non-maximum suppression: Non-maximum suppression is performed on the edge image obtained by the Canny algorithm to refine the edge; (3) Edge connection: edge connection is performed on the suppressed edges to fill the broken edge parts so that the edges become complete lines; (4) Edge screening: Screen the detected edges according to specific parking space line features; (5) Output results: Mark the filtered parking space lines on the original image, or output them as an independent data structure for subsequent parking space recognition and detection.

6. The control method of the intelligent automatic parking control device using multi-sensor information fusion according to claim 2 is characterized in that: Step 3: Process the point cloud information collected by the point cloud information collection sensor, including data filtering and feature extraction, to obtain the position coordinates of the obstacle vehicle. The specific steps are: (1) Use Gaussian filtering to filter the point cloud data collected by the lidar to remove outliers and noise; (2) Extract the shape and location information of obstacles in the point cloud, that is, feature extraction. The specific steps are as follows: 1) Use the DBSCAN clustering algorithm to segment obstacles on the preprocessed lidar data; 2) Vehicle profile extraction and analysis: By further processing the clustered point cloud data, points representing the vehicle contour are extracted; the specific steps are: a. Calculate the convex hull of the point cloud data to obtain a convex polygon representing the vehicle shape; b. Whether the convex hull represents the vehicle contour can be determined based on the geometric features such as the area and perimeter of the convex hull; c. Further analyze the shape and orientation of the convex hull to determine the position and orientation of the vehicle.

7. The control method of the intelligent automatic parking control device using multi-sensor information fusion according to claim 2 is characterized in that: In step 4, based on the data provided in steps 2 and 3, the unscented Kalman filter (UKF) is used to fuse the preprocessed image information and point cloud information to improve the accuracy and robustness of parking space recognition and positioning. The specific steps are as follows: (1) System modeling: Model the vehicle position and state by defining state variables and state transition equations. State variables include the vehicle's position, speed, direction, and other information. The state transition equation describes how the state changes over time. The state equation is: X(t+1)=f(x(t),W(t))(4) Where, for different times t, W(k) is the Gaussian white noise of the random variable X, and f is the nonlinear state equation function; (2) Observation model: Define observation variables and observation equations; image information and point cloud information provide observations on the position, direction and parking space line of the obstacle vehicle; the observation equation is: Z(t)=h(x(t),V(t))(5) Where V(t) is the Gaussian white noise of the observation variable Z, and h is the observation equation of the nonlinear system. (3) Unscented Kalman Filter UKF initialization: Initialize the state estimate and covariance matrix of the unscented Kalman filter UKF; the initial state is obtained based on prior information or sensor measurement, where the prior information refers to: a. The initial state of the system: The initial state includes the initial position, speed, and direction; b. Dynamic characteristics of the system: System dynamics include motion model, acceleration, and turning rate; c. Environmental conditions and constraints: Information about environmental conditions and constraints of the automated parking vehicle; (4) Prediction step: a. Status prediction: According to the state transfer equation, the unscented Kalman filter UKF is used to predict the state and estimate the vehicle state and state covariance at the next moment; b. Covariance prediction: After the state prediction, the covariance matrix of the system state, i.e., the uncertainty of the system state, is estimated; the covariance matrix is ​​updated using the predicted state estimate and related parameters to reflect the prediction error and uncertainty; (5) Update steps: a. Measurement prediction: First, the predicted state is converted into predicted observation value using the observation model of the system; the observation model is used to describe the relationship between the state and the observation; then, the predicted observation value of the system is estimated based on the predicted state and the observation model; b. Calculate the Kalman gain: The calculation formula of Kalman gain is: Where P is the covariance matrix of the system; c. State update: Use measurement predictions, Kalman gains, and actual observations to update the system's state estimate; The formula for updating the covariance matrix of the state of a nonlinear system is: (6) Fusion: A weighted average strategy is used to fuse image information and point cloud information to obtain a comprehensive and accurate vehicle state estimation; (7) Repeated Iteration: Iterate the prediction and update steps to continuously track the vehicle status and integrate new observation information.

Images