Dynamic Prediction and Obstacle Avoidance Method for the Visual Blind Area of a Double Semi-trailer Truck Train during Small-curvature Turning

Abstract

The present invention belongs to the field of road vehicle control, and relates to a method for dynamically predicting and avoiding obstacles in the vision blind area during small-curvature turning of a double semi-trailer truck train. The method constructs a dynamic blind area model based on the contour projections of the first and second semi-trailers at time k, the ground visible area observed by the driver through the rearview mirror, and the basic visible areas of the first and second semi-trailers; expands the blind area and sets it as the region of interest, processes the point cloud data within the region of interest to predict obstacle information; predicts the future vehicle state using the MPC algorithm based on the obtained vehicle state, and combines the blind area predicted by the dynamic blind area model and the predicted obstacle contour and position to judge the existing risks, and rolls and optimizes the control quantity, always keeping the safety distance between the blind area contour and the obstacle not less than a predetermined threshold, establishing a closed-loop safety protection system of perception-prediction-decision-execution, shortening the risk response time, and improving the safety and stability of vehicle driving.

Description

Technical Field

[0001] The present invention belongs to the field of road vehicle control, and relates to predicting or avoiding possible or upcoming collisions. Specifically, it relates to a method for dynamically predicting the vision blind area and avoiding obstacles during small-curvature turning of a double semi-trailer truck train. Background Art

[0002] With the continuous expansion of the demand for long-distance transportation, in order to ensure the safety and stability of large freight vehicles during driving, double semi-trailer truck trains have been trial-operated in many areas. However, due to the complex body structure, the dynamic blind area generated by the multi-section articulated structure during turning is time-varying, and the existing real-time blind area detection technology has been difficult to meet the safety requirements. There is a need to develop a blind area prediction and obstacle avoidance intervention method for double semi-trailer truck trains. By predicting the dynamic evolution of the blind area during the turning process of the vehicle in real time and combining active intervention strategies, the collision risk caused by the change of the blind area can be reduced, more sufficient warning information can be provided for the driver, and the road safety level can be improved.

[0003] In the prior art, the detection methods for the turning blind area of vehicles usually adopt two methods: one is to use fixed sensors and warning devices installed on the roadside to monitor the turning lane and the preset blind area in real time; the other is to use sensors installed on the vehicle body to monitor the blind area.

[0004] For the system adopting the roadside fixed installation scheme, once an obstacle is detected to enter the preset blind area, a warning is issued through sound and light signals. For example, Chinese patents CN 118471018 A and CN 111369828 A adopt this method. However, due to the rapid change of the posture of the trailer during the turning process, this passive detection method lacks dynamic prediction ability and cannot predict the evolution of the blind area with the change of the posture of the tractor and the trailer; at the same time, due to the sensors must be fixedly installed, it is difficult to deploy this scheme on a large scale (such as in mountain roads and other environments). Due to limited installation conditions, its popularity and economy in practical applications are not high. And the system only provides the function of sound and light prompt and does not have the ability of active intervention, resulting in that the driver still needs to perform emergency operations by himself after discovering the warning. Not only is the reaction time relatively long, but also in case of emergencies, it is easy to have problems such as improper control or excessive operation, further increasing the risk.

[0005] For a system adopting a vehicle body installation solution, Chinese Patent CN 113205704 A installs cameras on the side of the vehicle and demarcates a warning line around the vehicle body at a fixed distance. When the vehicle is stationary, the blind spot range is calibrated according to the warning line, and if an obstacle is detected to enter this range during driving, a warning is triggered. However, when the articulated angle of the trailer changes, the preset static blind spot is no longer the real blind spot area, and it is impossible to capture the evolution of the blind spot caused by the change of the trailer attitude, resulting in inaccurate determination of the blind spot range. In addition, this system can only send a prompt message after detecting an obstacle and cannot actively intervene, providing limited actual help to the driver, and the overall safety guarantee ability is still insufficient. Chinese Patent CN 119037446 A provides a blind spot detection method and system for heavy trucks. When detecting the blind spot, it does not use a dynamic prediction algorithm to estimate the future trajectory of the vehicle, but directly gives a fixed trajectory as the judgment basis according to the steering information obtained in real time and the calculated turning radius. In addition, this patent uses the vehicle centroid trajectory instead of the vehicle contour to check for blind spot conflicts, ignoring the dynamic projection of the vehicle body contour, resulting in inaccurate determination of the safety boundary and limitations in risk assessment.

[0006] In summary, the existing detection methods are limited by the static blind spot model and discretized perception, making it difficult to predict the dynamic evolution of the blind spot under the combined action of multiple degrees of freedom such as the vehicle articulated angle and yaw angle. Moreover, the risk judgment is based on the conflict between the vehicle centroid coordinates and wheel trajectories and obstacles, and the conflict between the vehicle outer contour and obstacles cannot be included in the assessment. At the same time, the system only stops at warning prompts and is not coupled with vehicle dynamics control, so it is impossible to achieve active risk avoidance. Summary of the Invention

[0007] In view of the above technical problems and deficiencies, the present invention provides a method for dynamically predicting and avoiding obstacles in the vision blind spot during small-curvature turning of a double semi-trailer truck train. This method constructs a dynamic blind spot model based on the contour projections of the first and second semi-trailers at time k, the ground visible area observed by the driver through the rearview mirror, and the basic visible areas of the first and second semi-trailers; predicts the future vehicle state using the MPC algorithm according to the obtained vehicle state, and combines the blind spot predicted by the dynamic blind spot model and the predicted contour and position of the obstacle to judge the existing risks, and roll-optimizes the control quantity to always keep the safety distance between the blind spot contour and the obstacle not less than a predetermined threshold, establishing a closed-loop safety protection system of perception-prediction-decision-execution, shortening the risk response time, and improving the safety and stability of vehicle driving.

[0008] To achieve the above object, the present invention adopts the following technical solutions:

[0009] A method for dynamically predicting and avoiding obstacles in the vision blind spot during small-curvature turning of a double semi-trailer truck train, the method comprising the following steps:

[0010] Step S1: Obtain the state parameters of the vehicle, establish a single-track dynamics model for the right turn of a double semi-trailer truck train, and derive and discretize it to obtain the discrete state equation and output equation in space;

[0011] Step S2: Establish a dynamic blind area model for the double semi-trailer truck train, and predict the blind area through the dynamic blind area model; among them, the dynamic blind area model of the double semi-trailer truck train is constructed based on the contour projections of the first and second semi-trailers at time k, the ground visible area observed by the driver through the rearview mirror, and the basic visible areas of the first and second semi-trailers;

[0012] Step S3: Expand the blind area predicted in Step S2 and set it as the region of interest, use lidar to detect obstacles, obtain point cloud data, and process the point cloud data in the region of interest to predict obstacle information, where the obstacle information includes the obstacle contour and position;

[0013] Step S4: Based on the state equation and output equation in space, use the MPC algorithm (trajectory tracking control algorithm) to predict the future vehicle state, and combine the blind area predicted by the dynamic blind area model and the predicted obstacle contour and position to judge the existing risks, and roll and optimize the control amount to always keep the safety distance between the blind area contour and the obstacle not less than the predetermined threshold.

[0014] As a preference of the present invention, the expression of the dynamic blind area model of the double semi-trailer truck train in Step S2 is:

[0015] ;

[0016] Among them, represents the blind area, represents the basic visible area of the first semi-trailer, represents the basic visible area of the second semi-trailer, represents the ground visible area observed through the rearview mirror, represents the contour projections of the two semi-trailers.

[0017] As a preference of the present invention, the contour projection of the first semi-trailer at time k in Step S2 is:

[0018] = ({( , ),( , ),( , ),( , )});

[0019] Among them, is the Contour projection of the first semi-trailer at the time step; is the convex hull calculation function, ([[]] , ) is the left front corner point of the first semi-trailer, ([[]] , ) is the right front corner point of the first semi-trailer, ([[]] , ) is the left rear corner point of the first semi-trailer, ([[]] , ) is the right rear corner point of the first semi-trailer;

[0020] Contour projection of the second semi-trailer at time k:

[0021] = ({{([[]] , ),([[]] , ),([[]] , ),([[]] , )});

[0022] Among them, is the Contour projection of the second semi-trailer at the time step; is the convex hull calculation function, ([[]] , ) is the left front corner point of the second semi-trailer, ([[]] , ) is the right front corner point of the second semi-trailer, ([[]] , ) is the left rear corner point of the second semi-trailer, ([[]] , ) is the right rear corner point of the second semi-trailer;

[0023] Contour projections of the first and second semi-trailers are:

[0024] ({ });

[0025] Among them, The Contour projection of the two semi-trailers at the time step; is the convex hull calculation function.

[0026] As a preference of the present invention, the visible ground area observed by the driver through the rearview mirror at time k in step S2 is a sector with , as the center and as the radius, and its starting angle and ending angle are respectively and constitute a region; wherein, is the abscissa of the center position of the rearview mirror in the global coordinate system, is the ordinate of the center position of the rearview mirror in the global coordinate system, is the height of the center position of the rearview mirror in the global coordinate system;

[0027] Taking the center position of the rearview mirror in the global coordinate system as the vertex of the cone, the field of view of the rearview mirror is regarded as a pyramid, and its central axis direction is , the horizontal half-angle is , and the vertical half-angle is γ ;

[0028] and are the left and right boundary angles of the pyramid in the horizontal direction, and the expression is:<X1 = x0 + d * cos(θ + α)>

[0029] ;<X2 = x0 + d * cos(θ - α)>

[0030] ;<Y1 = y0 + d * sin(θ + α)>

[0031] ;<Y2 = y0 + d * sin(θ - α)>

[0032] wherein, is the farthest visible distance.

[0033] As a preference of the present invention, the method for determining the basic visible regions of the first and second semi-trailers at time step k in step S2 is: calculating the basic visible region when the vehicle is driving straight according to the method for calculating the ground visible region observed by the driver through the rearview mirror at time step k , and then dividing it into two sub-regions , according to the front and rear boundaries of the first and second semi-trailers. Subsequently, at each time step , according to the articulation angle between the tractor and the first semi-trailer and the articulation angle between the first semi-trailer and the second semi-trailer, adjust the positions of the two sub-regions and update them to [[ID=५८]]、 , represents the basic visible region of the first semi-trailer, represents the basic visible region of the second semi-trailer.

[0034] As a further preference of the present invention, step S3 specifically includes the following steps:

[0035] Step S31: Obtain point cloud data and preprocess it;

[0036] Step S32: According to the blind area predicted in Step S2, expand 1 meter outward according to its contour shape and set it as the region of interest. Only the point cloud located within the region of interest is retained to obtain a more focused point cloud;

[0037] Step S33: Divide the point cloud into multiple uniform small grids. Each grid is a Pillar, and it is encoded as , , ; where represents the number of selected Pillars, is the maximum number of point clouds stored in each Pillar; is the attribute of the point cloud;

[0038] Step S34: Convert the tensorized point cloud into a tensor of size , , through the BatchNorm and ReLu functions; then, perform max pooling operation according to the dimension where the Pillar is located, that is, a feature map of dimension , is obtained; convert P to (H, W), and finally obtain a pseudo-image of , , ; where represents the number of feature channels, and H and W are the height and width of the picture respectively; then, input the pseudo-image into a 2D convolutional network for feature extraction; finally, the 2D convolutional network will output feature maps of different scales;

[0039] Step S35: Use the SSD method for 3D object detection; finally, post-process the preliminary detection results through non-maximum suppression, and output the detected object information, including the projection contour of the object on the ground and position.

[0040] As an optimization of the present invention, in Step S4, first construct the deviation of the vehicle's state quantity and the deviation of the input quantity, update the discrete space state equation in Step S1 to obtain a new discrete space state equation and output equation; then construct a new state vector, and reconstruct the reconstructed space state equation and output equation :

[0041] ;

[0042] where , = , = , is the input vector at time k; the discrete state matrix is and the input matrix is and the discrete-time output matrix is , and the new state vector is ;

[0043] After that, the future step state sequence is recursively calculated according to the reconstructed space state equation.

[0044] As a further preference of the present invention, the four-corner coordinates of the first semi-trailer at time k are calculated as follows:

[0045] ;

[0046] ;

[0047] ;

[0048] where the rotation matrix is , is the abscissa of the centroid of the first semi-trailer in the global coordinate system; is the ordinate of the centroid of the first semi-trailer in the global coordinate system; is the heading angle of the first semi-trailer, ; is the distance from the centroid of the first semi-trailer to the front boundary, is the distance from the centroid of the first semi-trailer to the rear boundary, is the width of the first semi-trailer; is the yaw angle of the tractor, is the articulation angle between the tractor and the first semi-trailer.

[0049] As a further preference of the present invention, when the predicted time step is greater than the input time step in step S4, the expression of the output equation is limited by the input time step. Therefore, the expression of the final predicted output model is:

[0050] ;

[0051] where, , , represents the control input increment, and the new state vector , and are the increments of the state quantity and the control input quantity respectively, is the number of steps of the predicted output vector, is the number of steps of the input vector.

[0052] As a further preference of the present invention, in step S4, the MPC algorithm solves the constrained QP problem iteratively within each sampling period to obtain the optimal control increment.

[0053] , , ;

[0054] ;

[0055] wherein, , , = ; is the predicted output value of the system; is the output reference value; is the system control increment; and are the weighting matrices of the system output and control increment respectively; is the penalty weight for the slack variable, taking 1× ; is the slack variable; is the lower limit of the control input; is the upper limit of the control input; is the lower limit of the control input increment; is the upper limit of the control input increment; is the point in the dynamic blind area ; is the point on the obstacle contour ; is the minimum safety distance between the blind area and the obstacle, is the control input at the k+i step; is the safety distance between the blind area and the obstacle at the k+i step.

[0056] Advantages and beneficial effects of the present invention:

[0057] (1) Aiming at the problem that the existing double semi-trailer vehicle train can only detect the static blind area and lacks the prediction of the dynamic blind area during future driving, the present invention obtains key attitude parameters such as the articulation angle and yaw angle in real time by establishing a four-degree-of-freedom dynamics model, analyzes the pose change law during the vehicle turning process, dynamically updates the spatio-temporal position relationship between the visible area and the occlusion area of the rearview mirror, and constructs a dynamic blind area prediction model based on the evolution of the motion state.

[0058] (2) In order to improve the efficiency and accuracy of blind area obstacle recognition, the present invention uses the predicted dynamic blind area expansion area as the region of interest (ROI) of the point cloud, and only performs object detection on the point cloud within the ROI, reducing the amount of irrelevant data processing, so that the blind area risk can be detected and actively intervened in a timely manner within the controller sampling period.

[0059] (3) To address the issue that existing collision avoidance solutions only use simple geometric intersections between the left and right tire trajectories and obstacles to judge collision risks, ignoring the overlap between the vehicle contour and the blind spot contour with obstacles, a new MPC objective function is constructed. A minimum distance constraint between the vehicle and the blind spot contour and obstacles is added, and a slack variable is introduced. When the predicted contour overlaps with the obstacle contour, the objective function will set an extremely high cost weight for this slack quantity, so as to automatically adjust the control quantity during the optimization process, keeping the entire vehicle body and the blind spot contour rather than the tire trajectory outside the safety boundary. Description of the Drawings

[0060] By referring to the following description in conjunction with the drawings, and with a more comprehensive understanding of the present invention, other objects and results of the present invention will become clearer and easier to understand. In the drawings:

[0061] FIG. 1 is a flowchart of a method for dynamically predicting the vision blind spot and avoiding obstacles during small-curvature turning of a double semi-trailer truck train according to the present invention;

[0062] FIG. 2 is a schematic diagram of a single-track dynamics model for a right turn of a double semi-trailer truck train;

[0063] FIG. 3 is a schematic diagram of the basic visible area of a double semi-trailer truck train;

[0064] FIG. 4 is a schematic diagram of the vision blind spot during turning of a double semi-trailer truck train. Detailed Embodiments

[0065] To enable those skilled in the art to better understand the technical solutions and advantages of the present invention, the present application will be described in detail below with reference to the drawings, but it is not used to limit the protection scope of the present invention.

[0066] As Figure 1 shown, this embodiment provides a method for dynamically predicting the vision blind spot and avoiding obstacles during small-curvature turning of a double semi-trailer truck train. By constructing a predictive control framework based on a single-track dynamics model of a right turn of a double semi-trailer truck train with four degrees of freedom of the vehicle, the prediction of the spatio-temporal evolution of the blind spot is realized, and the optimal obstacle avoidance steering angle is generated in combination with the model predictive control (MPC) algorithm, establishing a closed-loop safety protection system of perception - prediction - decision - execution, shortening the risk response time, and improving the safety and stability of vehicle driving.

[0067] In this embodiment, the double semi-trailer truck train consists of a tractor and two semi-trailers, that is, a total of three vehicle units. The vehicle units are connected by a saddle, and there is a hinge point on the saddle. The hinge point E is between the tractor and the first semi-trailer, and the hinge point F is between the first semi-trailer and the second semi-trailer.

[0068] Specifically, the method for dynamically predicting the blind spot and avoiding obstacles includes the following steps:

[0069] Step S1: Obtain the state parameters of the vehicle, establish a single-track dynamic model of a double-trailer vehicle train turning right, and derive and discretize to obtain a discrete spatial state equation and output equation;

[0070] Step S2: establishing a dynamic blind spot model for a double semi-trailer train, and predicting the blind spots using the dynamic blind spot model; wherein the dynamic blind spot model for the double semi-trailer train is constructed based on the outline projections of the first and second semi-trailers at time k, the ground visible area observed by the driver through the rearview mirror, and the basic visible areas of the first and second semi-trailers;

[0071] Step S3: Expand the blind area predicted in step S2 and set it as a region of interest. Use the lidar to detect obstacles, obtain point cloud data, and process the point cloud data within the region of interest to predict obstacle information, including obstacle outline and location.

[0072] Step S4: Based on the spatial state equation and the output equation, the MPC algorithm (trajectory tracking control algorithm) is used to predict the future vehicle state, and the blind spot predicted by the dynamic blind spot model and the predicted obstacle outline and position are combined to determine the existing risk, and the control amount is optimized in a rolling manner to always maintain the safe distance between the blind spot outline and the obstacle not less than the predetermined threshold. In this embodiment, step S1 constructs a right-turning single-track dynamic model of a double semi-trailer vehicle train based on the vehicle's four degrees of freedom, where the degrees of freedom are: the lateral speed of the tractor, ... , the yaw angular velocity of the tractor , the first articulation angular velocity , the second articulation angular velocity ;like Figure 2 As shown, the specific process of establishing the right-turning single-track dynamic model of the double-trailer vehicle train is as follows:

[0073] Step S11: Obtain vehicle status parameters, including: the mass of the tractor, the first and second semi-trailers 、 、 ; lateral speed of the three vehicle units 、 、 and longitudinal speed ; Center of mass velocity 、 、 ; Lateral acceleration of center of mass 、 、 ; Yaw angle 、 、 ; and take its derivative to get the yaw angular velocity , , ; Moment of inertia , , ; Steering angle of the front wheels of the tractor ; Sideslip angles of the centers of mass of the three vehicle units , , ; Articulation angle between the tractor and the first semi-trailer and angular velocity ; Articulation angle between the first semi-trailer and the second semi-trailer <s and angular velocity ; Distances from the center of mass of the tractor to the front axle, rear axle, and articulation point , and ; Distances from the center of mass of the first semi-trailer to the front axle, rear axle, and articulation point , and ; Distances from the center of mass of the second semi-trailer to the front axle, rear axle, and articulation point , and ;

[0074] Step S12: Solve the lateral accelerations of the centers of mass of the vehicle units. The specific expressions are as follows:

[0075] Lateral acceleration of the center of mass of the tractor:

[0076] ;

[0077] Assuming β is small, we can obtain , and substituting it into the previous equation gives:

[0078] ;

[0079] Among them, is the derivative of the lateral velocity of the tractor;

[0080] Similarly, the lateral accelerations of the centers of mass of the first and second semi-trailers are:

[0081] ;

[0082] Step S13: Solve the lateral forces of the tires on each axle. According to the side-slip characteristics of linear tires, the lateral forces of the tires on each axle are:

[0083] ;

[0084] Among them, are the side-slip stiffnesses of the front wheels of the tractor, rear wheels of the tractor, tires of the first semi-trailer, and tires of the second semi-trailer, respectively;

[0085] Furthermore, the side slip angle of the front wheels of the tractor is:

[0086] ;

[0087] In the formula, and are the actual lateral speed and longitudinal speed of the front wheels respectively. Assuming that the steering angle is very small, there is:

[0088] ;

[0089] In the formula, and are the lateral speed and longitudinal speed of the front wheels in the coordinate system of the tractor. Their relationships with the lateral and longitudinal speeds of the tractor are:

[0090] ;

[0091] Substituting the above two sets of equations into the formula for the side slip angle of the front wheels and simplifying, we get:

[0092] ;

[0093] Since , it is simplified to: ;

[0094] Similarly, the side slip angle of the rear wheels of the tractor is: = ;

[0095] The side slip angles of the rear axle tires of the first and second semi-trailers are: ;

[0096] Step S13: Isolate each vehicle unit for analysis. The equilibrium equations of each vehicle unit are obtained from the lateral force balance and yaw moment balance:

[0097] The equilibrium equations of the tractor:

[0098] ;

[0099] The equilibrium equations of the first semi-trailer:

[0100] ;

[0101] The equilibrium equations of the second semi-trailer:

[0102] ;

[0103] Among them, and are the lateral forces of the front and rear wheels of the tractor respectively; is the lateral force of the first semi-trailer tire; is the lateral force of the second semi-trailer tire; and is E 、 F the lateral forces on the two articulation points, each being a pair of action and reaction forces; 、 、 are the yaw angular velocities 、 、 derivatives;

[0104] Step S14: Combine the constraint conditions of the articulation points: , and simplify the above equations by combining them to obtain the single-track dynamic model of a double semi-trailer truck during a right turn;

[0105] Specifically, in order to further derive the single-track dynamic model of the double semi-trailer truck during a right turn into the state space equation used later, the following dynamic equations are given:

[0106] ;

[0107] Step S15: Let the state variables , the observed variables , the control input variables , simplify the above differential equations to retain only the variables, their first-order derivatives, and second-order derivatives in the state variables , and write them in matrix form ;

[0108] ;

[0109] ;

[0110] Among them, the elements in the matrix are as follows:

[0111] ;

[0112] ;

[0113] ;

[0114] ;

[0115] ;

[0116] ;

[0117] ;

[0118] The state - space equation in the space domain is obtained through matrix operations:

[0119]

[0120] wherein, is the state variable the first - order derivative of ; ; (is a 6 - order identity matrix);

[0121] Step S16: Derive and discretize the state - space equation and the output equation;

[0122] Since model predictive control (MPC) is a control method based on discrete systems, the state - space equation in continuous time needs to be discretized first. Specifically, the zero - order hold method (ZOH) is used to discretize the state - space equation in continuous time. It is assumed that the input remains unchanged within each sampling period, that is, within the interval , is a constant. Let the sampling period be , after discretization using ZOH, the discretized state - space equation and output equation (observation equation) are:

[0123] ;

[0124] wherein, represents the discrete - state matrix , input matrix and discrete - time output matrix are defined as:

[0125] ;

[0126] wherein, ; is the integration variable;

[0127] Furthermore, in this embodiment, the process of establishing the dynamic blind - spot model of the double - semi - trailer truck - train in step S2 is specifically as follows:

[0128] <000,0540>Step S21: First, according to the relevant regulations of "Performance and Installation Requirements for Indirect Vision Devices of Motor Vehicles" and "Driver's Operating Position Dimensions of Goods Vehicles", determine the driver's eye point position: Two points, each offset 32.5 mm horizontally to the left and right at 635 mm above the seat, are used as the driver's eye points. When the vehicle is driving straight, the range that can be observed through the rearview mirror from this eye point is defined as the "basic visible area". When the vehicle is turning, due to the change in the articulation angle between the semi-trailer and the tractor, the position of the semi-trailer changes, but the inherent visual field range of the rearview mirror remains unchanged, resulting in some areas that were originally within the "basic visible area" exceeding the visible range of the rearview mirror. This exceeded part and the projection of the vehicle onto the ground are defined as the visual blind area;

[0129] Step S22: Obtain the initial data;

[0130] Obtain the sampling time , the lateral speed and longitudinal speed of the tractor at the nth time step, as well as the yaw angle and yaw angular velocity , the articulation angle and angular velocity between the tractor and the first semi-trailer, , ; among them, is the distance from the centroid of the semi-trailer to the front boundary, is the distance from the centroid of the semi-trailer to the rear boundary, and the width of the two semi-trailers;

[0131] Step S23: After the data preparation is completed, perform the dynamic projection prediction of the semi-trailer contour;

[0132] First, predict the centroid coordinates of the tractor at the

[0133] nth time step:

[0134] Subsequently, calculate the saddle coordinates of the tractor. In the tractor coordinate system, the offset of the saddle relative to the centroid is ( , ). After rotating and translating it to the global coordinate system, we get:

[0135] ;

[0136] Among them, is the abscissa of the tractor saddle in the global coordinate system; is the ordinate of the tractor saddle in the global coordinate system; ; is the distance from the center of mass of the tractor to the articulation point;

[0137] Next, calculate the heading angle of the first semi-trailer , the offset of the center of mass of the first semi-trailer relative to the tractor saddle in the semi-trailer coordinate system is ( , ), rotate and translate it to the global coordinate system:

[0138] ;

[0139] in, is the horizontal coordinate of the center of mass of the first semi-trailer in the global coordinate system; is the ordinate of the center of mass of the first semi-trailer in the global coordinate system; ; is the distance from the center of mass of the first semi-trailer to the front axle;

[0140] Known trailer length and width , define the four-corner offset matrix in the first semi-trailer coordinate system as:

[0141] ;

[0142] Rotate and translate the four coordinates:

[0143] ;

[0144] in, , whose column vectors correspond to the global coordinates of the four corner points of the first semi-trailer; ( , ) is the left front corner point of the first semi-trailer, ( , ) is the right front corner point of the first semi-trailer, ( , ) Left rear corner point of the first semi-trailer, ( , ) is the right rear corner point of the first semi-trailer; is the four-corner offset matrix in the first semi-trailer coordinate system; is the horizontal coordinate of the center of mass of the first semi-trailer in the global coordinate system; is the ordinate of the center of mass of the first semi-trailer in the global coordinate system;

[0145] In the dynamic projection prediction of a vehicle, the ConvexHull function is used to calculate the minimum convex polygon of a set of points. The convex hull refers to the outer boundary of a set of points, which can form a closed shape without any concave parts. In this embodiment, the contour projection of the first semi-trailer at the time step is obtained by using the convex hull calculation as follows:

[0146] = ({( , ),( , ),( , ),( , )});

[0147] where, is the contour projection of the first semi-trailer at the time step; is the convex hull calculation function;

[0148] Similarly, after calculating the global coordinates of the saddle of the first semi-trailer, the contour projection of the second semi-trailer at the time step is obtained according to the above steps:

[0149] = ({{( , ),( , ),( , ),( , )});

[0150] where, is the contour projection of the second semi-trailer at the time step; is the convex hull calculation function;

[0151] Merge the envelopes (contour projections) of the two semi-trailers:

[0152] ({ })

[0153] where, is the contour projection of the two semi-trailers at the time step; is the convex hull calculation function;

[0154] Through the above steps, the occlusion area of the double semi-trailer combination at any moment can be constructed, providing basic support for subsequent blind spot determination.

[0155] Step S24: To construct the vision model of the tractor rearview mirror, realize the dynamic determination of the blind spot, and combine the motion state of the vehicle during turning, project this vision onto the ground to obtain the actual observable area (ground visible area) of the rearview mirror.

[0156] In this embodiment, based on the vehicle motion information predicted by MPC and the installation parameters of the rearview mirror, a rearview mirror vision cone model is established, and the ground visible range calculation in the global coordinate system is realized.

[0157] First, obtain the direction angle of the rearview mirror mirror surface normal relative to the tractor coordinate system axis; the horizontal viewing angle range of the rearview mirror ; the downward viewing angle of the driver relative to the center horizontal plane of the mirror surface observed from the rearview mirror , and the local coordinates of the center position of the rearview mirror in the tractor coordinate system γ , , , ;

[0158] Among them, is the abscissa of the center position of the rearview mirror in the tractor coordinate system, is the ordinate of the center position of the rearview mirror in the tractor coordinate system, is the height of the center position of the rearview mirror in the tractor coordinate system;

[0159] Convert it to the global coordinate system to get:

[0160] ;

[0161] Among them, is the abscissa of the center position of the rearview mirror in the global coordinate system, is the ordinate of the center position of the rearview mirror in the global coordinate system, is the height of the center position of the rearview mirror in the global coordinate system; , is the rotation matrix in step S23;

[0162] At the same time, the direction angle of the line of sight direction (central axis) of the rearview mirror in the global coordinate is:

[0163] ;

[0164] Taking as the cone vertex, the rearview mirror vision can be regarded as a pyramid, and its central axis direction is , the horizontal half angle is , and the vertical half angle is γ ;

[0165] In the horizontal direction, the left and right boundary angles of the pyramid are respectively:

[0166] ;

[0167] Among them, is the left boundary angle of the field of view cone; is the right boundary angle of the field of view cone; is the line of sight direction angle of the rearview mirror in the global coordinate system; is the horizontal viewing angle range of the rearview mirror;

[0168] Starting from the center position of the rearview mirror, the three-dimensional parametric expressions of the two boundary rays in the cone are:

[0169] ;

[0170] In the formula, is the length of the line of sight ray; are the left and right boundary angles of the cone; since the driver usually observes downward through the rearview mirror, so is a negative value.

[0171] To obtain the farthest visible distance on the ground, the intersection point of the required ray and the ground plane is required:

[0172] ;

[0173] Solve the parameter at the intersection point and project it onto the ground to obtain the farthest visible distance :

[0174] ;

[0175] Construct a sector with , as the center and as the radius. Its starting angle and ending angle are respectively and , and the formed area is the ground visible area observed by the driver through the rearview mirror at time k .

[0176] Step S25: At time step , use the result of step S24 to find the basic visible area, that is, the projection of the rearview mirror field of view cone onto the ground when the vehicle is moving straight , and then divide it into two sub-areas , , and then at each time step under, according to two hinge angles and adjust the positions of the two sub-regions and update them to , ;

[0177] Specifically, the coordinates of the i-th point in the sub-regions and are respectively , at each time step under, according to the changes of the hinge angles and , each sub-region will rotate;

[0178] For the i-th point in the sub-region :

[0179] ;

[0180] Among them, is the i-th point in the sub-region at the time step ; is the hinge angle between the tractor and the first semi-trailer at the time step , is the i-th point in the sub-region in the basic visible area;

[0181] For the i-th point in the sub-region :

[0182] ;

[0183] Among them, is the i-th point in the sub-region at the time step ; is the hinge angle between the first semi-trailer and the second semi-trailer at the time step ;

[0184] After the rotation transformation, the contours of the basic visible areas and can be regarded as the set of these transformed points, that is:

[0185] ;

[0186] ;

[0187] Among them, is the number of points in; For the number of midpoints;

[0188] Next, according to the yaw angle of the tractor Adjust the direction of the rearview mirror vision cone in the global coordinate system, and calculate the projection of the rearview mirror vision cone on the ground at the k time step (the visible area on the ground) ; Finally, Remove from the merged sub-region and then perform a union operation with the projection of the semi-trailer body contour to finally obtain the dynamic blind area at the current time step as follows:

[0189] .

[0190] Furthermore, in this embodiment, the specific process of obtaining obstacle information in step S3 is as follows:

[0191] Step S31: Obtain point cloud data and preprocess it to delete points with over-limit values, points that are too far away, points that are too high, and points on the vehicle itself.

[0192] Step S32: Obtain the blind area in the prediction result output in step S2 , expand it by 1 meter according to its contour shape and set it as the region of interest (ROI); perform geometric position judgment on the preprocessed remaining point cloud, and only retain the points located within the ROI to obtain a more focused point cloud for subsequent object detection and recognition, reducing the computational burden of irrelevant point data.

[0193] Step S33: Use the point cloud obtained after preprocessing and ROI filtering in steps S31 and S32 to divide the point cloud into multiple uniform small grids, with each grid being a Pillar, and encode it as , , ; where represents the number of selected Pillars, is the maximum number of point clouds stored in each Pillar; is the attribute of the point cloud, including the true coordinates and reflection intensity of the point cloud, the geometric center of all points in the Pillar, and the relative position of the point to the geometric center point; both P and N are hyperparameters that need to be set according to the number of beams of the lidar and the number of point clouds in the Pillar.

[0194] Step S34: Convert the tensorized point cloud into , , a tensor of size, and then, perform a max pooling operation according to the dimension where the Pillar is located, that is, a feature map of , dimensions is obtained; in order to obtain the pseudo-image features, P is converted to (H, W), and finally , , is obtained; where represents the number of feature channels, and H and W are the height and width of the image respectively. Next, input this pseudo-image into a 2D convolutional network (Backbone) for feature extraction; finally, the network will output feature maps of different scales to help detect objects of different sizes.

[0195] Step S35: The detection head uses the SSD method for 3D object detection. Specifically, on each feature map output by the Backbone network, a set of default boxes are predefined, and these default boxes cover target areas of different sizes, ratios, and orientations. Subsequently, SSD (Single Shot MultiBox Detector, a deep learning-based object detection algorithm) performs regression and classification predictions on all default boxes, and the output of each default box includes the offset of the center position, size change, orientation information, and the corresponding object category probability. This method enables the model to simultaneously complete object localization and classification in a single forward propagation, greatly improving the detection efficiency. Finally, post-process the preliminary detection results through non-maximum suppression (NMS), filter out duplicate boxes, and only retain the detection boxes with the highest confidence and less overlap. Finally, output the detected object information, including the projection contour of the object on the ground and position.

[0196] It should be noted that for the acquisition of obstacle information, those skilled in the art can also adopt existing methods. The technical solution provided by the present invention is only for illustrative purposes, and it is a preferred solution and does not limit the protection scope of the present invention.

[0197] Furthermore, in this embodiment, the specific process of predicting the future vehicle state using the MPC method and combining the blind area model to judge the existing risks and rolling and optimizing the control quantity in step S4 is as follows:

[0198] Step S41: In order to prevent the sudden change of the system control quantity from affecting its continuity, use the control increment to replace the control quantity, and construct the state quantity deviation and input quantity deviation of the vehicle:

[0199] , ;

[0200] where and They are the increments of the state quantity and the control input quantity respectively; , , , , They are the reference values of the original corresponding variables at time step respectively;

[0201] Substitute them into the discretized space state equation in step S16 to obtain the new discrete space state equation and output equation as follows:

[0202] ;

[0203] Construct a new state vector:

[0204] ;

[0205] Reconstruct the space state equation and the output equation :

[0206] ;

[0207] Among them, , = , = , is the input vector at time k;

[0208] Step S42: At each sampling moment, predict the vehicle trajectory based on the above-derived space state equation and roll-optimize the control quantity;

[0209] Specifically, recursively calculate the state sequence of the future steps according to the reconstructed space state equation in step S41 as the prediction process, and thus deduce the expansion formula of the system output :

[0210] ;

[0211] ;

[0212]

[0213] ;

[0214]

[0215] ;

[0216] According to the above derivation, define step prediction output vector and The step input vector is:

[0217] , ;

[0218] Since the prediction horizon is greater than the control horizon, when the predicted time step is greater than , the expression of the output equation will be restricted by Nc . Therefore, the exponent of the last term is not zero. Combining them, the system prediction output model can be obtained as:

[0219] ;

[0220] Among them, , ;

[0221] Step S43: The MPC controller solves the optimization problem at each sampling moment, enabling the system to not only closely follow the desired path to minimize the tracking deviation but also ensure smooth control input. Under the minimum distance constraint between the blind area and the obstacle, the vehicle is safely avoided through slack variables, and the safety distance between the blind area profile and the obstacle is always maintained not lower than the predetermined threshold;

[0222] Specifically, the design objective function is in the following form:

[0223] , , = ;

[0224] Among them, is the predicted output value of the system, is the output reference value, is the system control increment, and are the weighted matrices of the system output and control increment respectively, is the penalty weight for the slack variable, taking 1× , are the slack variables;

[0225] In addition, to ensure that the control action is smoothly applied within the physical range of the actuator and to ensure the safety distance between the blind area and the obstacle, a set of hard constraints and soft constraints need to be introduced throughout the prediction horizon;

[0226] Satisfy the control quantity constraint and control increment constraint within the prediction horizon:

[0227] , , , , ;

[0228] , , , , ;

[0229] wherein, is the lower limit value of the control input, is the upper limit value of the control input, is the lower limit value of the control input increment, is the upper limit value of the control input increment, is the control input quantity at the k + i-th step, is the control input increment at the k + i-th step;

[0230] Obtain the blind area contour at the step and the obstacle contour , and define the blind area - obstacle safety distance as:

[0231] ;

[0232] wherein, is a point in the dynamic blind area , is a point in the obstacle contour ;

[0233] The safety distance constraint is:

[0234] ;

[0235] wherein, is the minimum safety distance between the blind area and the obstacle;

[0236] Step S44: After the objective function and the constraints are established, the MPC trajectory tracking control algorithm based on the augmented state and the input increment needs to solve the following constrained QP problem (quadratic equation minimization problem with linear constraints) iteratively in each sampling period to obtain the optimal control increment;

[0237] , , ;

[0238] ;

[0239] Step S45: Repeat the above process in the next cycle.

[0240] The present invention also provides an electronic device, comprising: one or more processors and a memory; wherein, the memory is used for storing one or more programs, and when the one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned method for dynamically predicting and avoiding obstacles in the visual blind area during small-curvature turning of a double semi-trailer truck train.

[0241] The present invention also provides a computer-readable medium, on which a computer program is stored, and when the computer program is executed by a processor, the above-mentioned method for dynamically predicting and avoiding obstacles in the visual blind area during small-curvature turning of a double semi-trailer truck train is implemented.

[0242] Those skilled in the art can understand that all or part of the functions of the above-mentioned various methods / modules can be implemented in a hardware manner or in a computer program manner. When all or part of the functions in the above-mentioned embodiments are implemented in a computer program manner, the program can be stored in a computer-readable storage medium, and the storage medium can include: read-only memory, random access memory, magnetic disk, optical disk, hard disk, etc., and the above-mentioned functions are implemented by a computer executing the program. For example, the program is stored in the memory of the device, and when the program in the memory is executed by the processor, all or part of the above-mentioned functions can be implemented.

[0243] In addition, when all or part of the functions in the above-mentioned embodiments are implemented in a computer program manner, the program can also be stored in a storage medium such as a server, another computer, magnetic disk, optical disk, flash drive or mobile hard disk, downloaded or copied and saved to the memory of the local device, or the system of the local device is updated in version. When the program in the memory is executed by the processor, all or part of the functions in the above-mentioned embodiments can be implemented.

[0244] The above uses specific examples to elaborate on the present invention, which is only used to help understand the present invention and is not intended to limit the present invention. For those skilled in the technical field to which the present invention belongs, based on the idea of the present invention, several simple deductions, deformations or substitutions can also be made. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A method for dynamically predicting the vision blind area and obstacle avoidance of a double semi-trailer truck train during small-curvature turning, characterized in that, The method includes the following steps: Step S1: Obtain the state parameters of the vehicle, establish a single-track dynamics model for the right turn of a double semi-trailer truck train, and obtain the discrete space state equation and output equation after derivation and discretization; Step S2: Establish a dynamic blind area model for the double semi-trailer truck train, and predict the blind area through the dynamic blind area model; wherein, the dynamic blind area model of the double semi-trailer truck train is constructed based on the contour projections of the first and second semi-trailers at the k-th moment, the ground visible area observed by the driver through the rearview mirror, and the basic visible areas of the first and second semi-trailers; Step S3: Expand the blind area predicted in Step S2 and set it as the region of interest, use lidar to detect obstacles, obtain point cloud data, and process the point cloud data in the region of interest to predict obstacle information, where the obstacle information includes the obstacle contour and position; Step S4: Based on the space state equation and output equation, use the MPC algorithm to predict the future vehicle state, and combine the blind area predicted by the dynamic blind area model and the predicted obstacle contour and position to judge the existing risks, and roll-optimize the control quantity to always keep the safety distance between the blind area contour and the obstacle not less than the predetermined threshold.

2. The dynamic prediction and obstacle avoidance method for the small curvature turning vision blind area of a double semi-trailer truck train according to claim 1, characterized in that, The expression of the dynamic blind area model of the double semi-trailer truck train in Step S2 is: ； Among them, represents the blind area, represents the basic visible area of the first semi-trailer, represents the basic visible area of the second semi-trailer, represents the visible area of the ground observed through the rearview mirror, represents the contour projection of the two semi-trailers.

3. The dynamic prediction and obstacle avoidance method for the small-curvature turning vision blind area of a double semi-trailer truck train according to claim 1, characterized in that, The contour projection of the first semi-trailer at the k-th moment in Step S2 is: = ({( , ),( , ),( , ),( , )})； Among them, is the contour projection of the first semi-trailer at the time step; is the convex hull calculation function, ( , ) is the left front corner point of the first semi-trailer, ([[]] , ) is the right front corner point of the first semi-trailer, ([[]] , ) is the left rear corner point of the first semi-trailer, ([[]] , ) is the right rear corner point of the first semi-trailer; The contour projection of the second semi-trailer at the k-th moment: = ({{( , ),( , ),( , ),( , )})； Among them, is the contour projection of the second semi-trailer at the time step; is the convex hull calculation function, ( , ) is the left front corner point of the second semi-trailer, ([[]] , ) is the right front corner point of the second semi-trailer, ([[]] , ) is the left rear corner point of the second semi-trailer, ([[]] , ) is the right rear corner point of the second semi-trailer; The contour projections of the first and second semi-trailers are: ({ })； Among them, The outline projection of the two-and-a-half-trailer at the th time step; is the convex hull calculation function.

4. The dynamic prediction and obstacle avoidance method for the small-curvature turning vision blind area of a double semi-trailer truck train according to claim 1, characterized in that In step S2, the visible ground area observed by the driver at time k is a sector centered at , with a radius of . Its starting angle and ending angle are and respectively, forming a region. Among them, is the abscissa of the center position of the rearview mirror in the global coordinate system, is the ordinate of the center position of the rearview mirror in the global coordinate system, and is the height of the center position of the rearview mirror in the global coordinate system. Taking the central position of the rearview mirror in the global coordinate system as the vertex of the cone, the field of view of the rearview mirror is regarded as a pyramid, and its central axis direction is , the horizontal half-angle is , and the vertical half-angle is γ ; and are the left and right boundary angles of the pyramid in the horizontal direction, and the expression is: ; ; ； Among them, is the farthest visible distance.

5. The dynamic prediction and obstacle avoidance method for the small-curvature turning vision blind area of a double semi-trailer truck train according to claim 1, characterized in that The method for determining the basic visible areas of the first and second semi-trailers at time k in step S2 is as follows: Calculate the basic visible area during straight-line driving of the vehicle according to the method for calculating the ground visible area observed by the driver through the rearview mirror at time k. , and then divide it into two sub-areas according to the front and rear boundaries of the first and second semi-trailers. , , and then at each time step , according to the articulation angle between the tractor and the first semi-trailer and the articulation angle between the first semi-trailer and the second semi-trailer adjust the positions of the two sub-areas and update them to , , represents the basic visible area of the first semi-trailer, represents the basic visible area of the second semi-trailer.

6. The dynamic prediction and obstacle avoidance method for the small curvature turning vision blind area of a double semi-trailer truck train according to claim 1, characterized in that Step S3 specifically includes the following steps: Step S31: Obtain the point cloud data and preprocess it; Step S32: According to the blind area predicted in Step S2, expand it by 1 meter outward according to its contour shape and set it as the region of interest, and only retain the point cloud located within the region of interest to obtain a more focused point cloud; Step S33: Divide the point cloud into multiple uniform small grids, each grid being a Pillar, and encode it as , , ; where represents the number of selected Pillars, is the maximum number of point clouds stored in each Pillar; is the attribute of the point cloud; Step S34: Convert the tensorized point cloud into , , -sized tensors through the BatchNorm and ReLu functions; then, perform max pooling operations according to the dimension where the Pillars are located, that is, obtain , -dimensional feature maps; convert P to (H, W), and finally obtain , , pseudo-images; where represents the number of feature channels, and H and W are the height and width of the image respectively; then, input the pseudo-images into a 2D convolutional network for feature extraction; finally, the 2D convolutional network will output feature maps of different scales; Step S35: Perform 3D object detection using the SSD method; finally, post-process the preliminary detection results through non-maximum suppression and output the detected object information, including the projected contour of the object on the ground and position.

7. The dynamic prediction and obstacle avoidance method for the small-curvature turning vision blind area of a double semi-trailer truck train according to claim 1, characterized in that In step S4, first construct the deviation of the vehicle's state quantity and the deviation of the input quantity, update the discrete space state equation in step S1 to obtain a new discrete space state equation and output equation; then construct a new state vector and reconstruct the space state equation again and output equation : ； Among them, , = , = , is the input vector at time k; the discrete state matrix is , the input matrix is and the discrete-time output matrix is , and the new state vector is ; Then, recursively calculate the state sequence of the future steps according to the reconstructed spatial state equation.

8. The dynamic prediction and obstacle avoidance method for the small-curvature turning vision blind area of a double semi-trailer truck train according to claim 3, characterized in that The calculation method of the four - corner coordinates of the semi - trailer at time k is as follows: ； ； ； where the rotation matrix is , is the abscissa of the centroid of the first semi-trailer in the global coordinate system; is the ordinate of the centroid of the first semi-trailer in the global coordinate system; is the heading angle of the first semi-trailer, ; is the distance from the centroid of the first semi-trailer to the front boundary, is the distance from the centroid of the first semi-trailer to the rear boundary, is the width of the first semi-trailer; is the yaw angle of the tractor, is the articulation angle between the tractor and the first semi-trailer.

9. The dynamic prediction and obstacle avoidance method for the small-curvature turning vision blind area of a double semi-trailer truck train according to claim 7, characterized in that, In Step S4, when the predicted time step is greater than the input time step, the expression of the output equation is restricted by the input time step, so the expression of the final prediction output model is: ； Among them, , , represent the control input increment, and the new state vector , and are the increments of the state quantity and the control input quantity respectively, is the number of steps of the predicted output vector, is the number of steps of the input vector.

10. The dynamic prediction and obstacle avoidance method for the small curvature turning vision blind area of a double semi-trailer truck train according to claim 9, characterized in that, In Step S4, the MPC algorithm solves the constrained QP problem iteratively in each sampling period to obtain the optimal control increment; , , ； ； Among them, , , = ; is the predicted output value of the system; is the output reference value; is the system control increment; and are the weighting matrices of the system output and control increment respectively; is the penalty weight for the slack, taking 1× ; is the slack variable; is the lower limit value of the control input; is the upper limit value of the control input; is the lower limit value of the control input increment; is the upper limit value of the control input increment; is the point in the dynamic blind area . is the point on the obstacle contour . is the minimum safe distance between the blind area and the obstacle, is the control input at the (k + i)-th step; is the safe distance between the blind area and the obstacle at the (k + i)-th step.

Images