Dynamic prediction and obstacle avoidance method for small-curvature turning view blind area of double-semi-trailer train

Abstract

The invention belongs to the field of road vehicle control, and relates to a dynamic prediction and obstacle avoidance method for a small-curvature turning view blind area of a double-semi-trailer train. The method comprises the following steps: constructing a dynamic blind area model based on contour projections of a first section and a second section of semitrailer at a k moment, a ground visual area observed by a driver through rearview mirrors, and basic visual areas of the first section and the second section of semitrailer; expanding the blind area and setting the blind area as a region of interest, and processing the point cloud data in the region of interest to predict obstacle information; the future vehicle state is predicted through the MPC algorithm according to the obtained vehicle state, the existing risk is judged in combination with the blind area predicted by the dynamic blind area model and the predicted obstacle contour and position, the control quantity is optimized in a rolling mode, and the safe distance between the blind area contour and the obstacle is always kept not smaller than a preset threshold value; a perception-prediction-decision-execution closed-loop safety protection system is established, the risk response time is shortened, and the safety and stability of vehicle driving are improved.

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. It is necessary to develop a blind area prediction and obstacle avoidance intervention method for double semi-trailer truck trains. By real-time predicting the dynamic evolution of the blind area during the turning process of the vehicle 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, there are usually two methods for detecting the turning blind area of a vehicle: 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 an acoustic and optical signal. 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 the ability of dynamic prediction and cannot predict the evolution of the blind area with the changes 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 the limited installation conditions, the popularity and economy of its actual application are not high. And the system only provides an acoustic and optical prompt function 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 scheme, 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 area 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 articulation angle of the trailer changes, the preset static blind area is no longer the real blind area, and the evolution of the blind area caused by the change of the trailer attitude cannot be captured, resulting in inaccurate determination of the blind area range. In addition, after detecting an obstacle, this system can only send a prompt message and cannot actively intervene, providing limited practical help to the driver, and the overall safety guarantee ability is still insufficient. Chinese Patent CN 119037446 A provides a blind area detection method and system for heavy trucks. When detecting the blind area, 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 area 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 area model and discretized perception, making it difficult to predict the dynamic evolution of the blind area under the coupled action of multiple degrees of freedom such as the vehicle articulation 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 incorporated into the assessment. At the same time, the system stops at warning prompts and is not coupled with vehicle dynamics control, so active risk avoidance cannot be achieved. 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 area during small-curvature turning of a double semi-trailer truck train. This method constructs a dynamic blind area model based on the contour projections of the first and second semi-trailers at time k, the visible ground 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 area predicted by the dynamic blind area model and the predicted contour and position of the obstacle to judge the existing risks, and rolls and optimizes the control quantity to always keep 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.

[0008] To achieve the above object, the present invention adopts the following technical solutions: 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 comprising 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 derive and discretize to obtain the discrete space state equation and output equation; 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 (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-optimize the control quantity to always keep the safety distance between the blind area contour and the obstacle not less than a predetermined threshold.

[0009] 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: ; Wherein, 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.

[0010] As a preference of the present invention, the contour projection of the first semi-trailer at the k-th moment in Step S2 is: = ({( , ),( , ),( , ),( , )}); Wherein, is the contour projection of the first semi-trailer at the -th time step; is the convex hull calculation function, and ([[]] , ) 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 time step k: = ({{( , ),( , ),( , ),( , )}); Among them, is the contour projection of the second semi-trailer at the th 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 th time step of the contour projections of the two semi-trailers; is the convex hull calculation function.

[0011] As a preference of the present invention, the visible ground area observed by the driver through the rearview mirror at time step 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 constituting the area; 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; ​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: ; ; ; wherein, is the farthest visible distance.

[0012] As a preference of the present invention, the method for determining the basic visible areas of the first and second semi-trailers at the k-th moment in step S2 is: calculating the basic visible area during the straight driving of the vehicle according to the method for calculating the ground visible area observed by the driver through the rearview mirror at the k-th moment , and then dividing 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, adjusting the positions of the two sub-areas and updating them to , , represents the basic visible area of the first semi-trailer, represents the basic visible area of the second semi-trailer.

[0013] As a further preference of the present invention, step S3 specifically includes the following steps: Step S31: Obtain point cloud data and preprocess it; Step S32: According to the blind area predicted in step S2, expand it outward by 1 meter 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 is a Pillar, and encode it as , , ; wherein, 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 through the BatchNorm and ReLu functions into , , tensor of size; then, perform max pooling operation according to the dimension where the Pillar is located, that is, obtain , feature map of dimension; convert P to (H, W), and finally obtain , , pseudo-image; 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; 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.

[0014] As a preference of the present invention, in step S4, first construct the deviation of the state quantity and the deviation of the input quantity of the vehicle, update the discrete space state equation in step S1, and 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 : ; wherein, , = , = , 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 space state equation.

[0015] As a further preference of the present invention, the calculation method of the four-corner coordinates of the first semi-trailer at time k is: ; ; ; wherein, 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.

[0016] As a further preference of the present invention, when the predicted time step in step S4 is greater than the input time step, the expression of the output equation is restricted by the input time step. Therefore, the expression of the final predicted output model is: ; 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.

[0017] As an even 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; , , ; ; where, , , = ; 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 quantity and the control increment respectively; is the penalty weight for the slack variable, 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 dynamic blind area point in; is the point on the obstacle contour in; is the minimum safe distance between the blind area and the obstacle, is the control input quantity at the (k + i)-th step; is the safe distance between the blind area and the obstacle at the (k + i)-th step.

[0018] Advantages and beneficial effects of the present invention: (1) Aiming at the problem that the existing double semi-trailer truck train can only detect static blind areas and lacks the prediction of dynamic blind areas 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 rearview mirror visible area and the occlusion area, and constructs a dynamic blind area prediction model based on the evolution of the motion state.

[0019] (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 time within the controller sampling period.

[0020] (3) In order to solve the problem that the existing collision avoidance scheme only judges the collision risk by simply geometrically intersecting the left and right tire trajectories with the obstacle, ignoring the overlap between the vehicle contour and the blind area contour and the obstacle, the present invention constructs a new MPC objective function, adds the minimum distance constraint between the vehicle and the blind area contour and the obstacle and introduces a slack variable. When the predicted contour overlaps with the obstacle contour, the objective function will set a very high cost weight for this slack variable, so as to automatically adjust the control quantity during the optimization process, and keep the entire vehicle body and the blind area contour rather than the tire trajectory outside the safety boundary. Description of the Drawings

[0021] Through the following description with reference to the drawings, and with a more comprehensive understanding of the present invention, other objects and results of the present invention will become more obvious and easier to understand. In the drawings: Figure 1 is a flow chart of the dynamic prediction and obstacle avoidance method for the vision blind area of the double semi-trailer truck train of the present invention; Figure 2 is a schematic diagram of the single-track dynamics model of the double semi-trailer truck train turning right; Figure 3 is a schematic diagram of the basic visible area of the double semi-trailer truck train; Figure 4 is a schematic diagram of the turning vision blind area of the double semi-trailer truck train. Detailed Embodiments

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

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

[0024] In this embodiment, the double semi-trailer truck train is composed 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.

[0025] Specifically, the method for dynamically predicting and avoiding obstacles in the blind area includes the following steps: Step S1: Obtain the state parameters of the vehicle, establish a right-turn single-track dynamics model of the 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 of 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 kth moment, the visible ground 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 (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 quantity to always keep the safety distance between the blind area contour and the obstacle not less than a predetermined threshold. In this embodiment, in Step S1, a right-turn single-track dynamics model of the double semi-trailer truck train based on the four degrees of freedom of the vehicle is constructed, where the degrees of freedom are respectively: the lateral speed of the tractor and the yaw angular velocity of the tractor and the angular velocity of the first hinge , the second articulated angular velocity ; as Figure 2 shown, the specific establishment process of the right-turn single-track dynamic model of the double semi-trailer truck train is as follows: Step S11: Obtain the state parameters of the vehicle, specifically including: the masses of the tractor, the first and second semi-trailers , , ; the lateral velocities , , and the longitudinal velocities of the three vehicle units; the resultant velocity of the center of mass , , ; the lateral acceleration of the center of mass , , ; the yaw angle , , ; and take the derivative of it to obtain the yaw angular velocity , , ; the moments of inertia , , ; the front-wheel deflection angle of the tractor ; the sideslip angles of the centers of mass of the three vehicle units , , ; the articulation angle and the angular velocity between the tractor and the first semi-trailer; the articulation angle and the angular velocity between the first semi-trailer and the second semi-trailer; the distances from the center of mass of the tractor to the front axle, rear axle and articulation point , and ; the distances from the center of mass of the first semi-trailer to the front axle, rear axle and articulation point , and ; the distances from the center of mass of the second semi-trailer to the front axle, rear axle and articulation point , and ; Step S12: Solve the lateral acceleration of the center of mass of each vehicle unit, and the specific expressions are as follows: Lateral acceleration of the center of mass of the tractor: ; Assume that β is small, then we can get , substitute it into the previous formula to get: ; Among them, is the derivative of the lateral speed of the tractor; Similarly, the centripetal lateral accelerations of the first and second semi-trailers are: ; Step S13: Solve the lateral forces of the tires on each axle. According to the cornering characteristics of linear tires, the lateral forces of the tires on each axle are: ; Among them, are the cornering stiffnesses of the front wheels of the tractor, the rear wheels of the tractor, the tires of the first semi-trailer, and the tires of the second semi-trailer, respectively; Furthermore, the cornering angle of the front wheels of the tractor is: ; In the formula, , are the actual lateral speed and longitudinal speed of the front wheels respectively. Assuming that the steering angle is very small, there is: ; In the formula, , 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: ; Substitute the above two groups of equations into the formula for the cornering angle of the front wheels and simplify to get: ; Since , it is simplified to: ; Similarly, the cornering angle of the rear wheels of the tractor is: = ; The cornering angles of the rear-axle tires of the first and second semi-trailers are: ; 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: The equilibrium equations of the tractor: ; The equilibrium equations of the first semi-trailer: ; The equilibrium equations of the second semi-trailer: ; 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 are E , F the lateral forces received by the two hinge points, each being a pair of action and reaction forces; , , are the yaw angular velocity , , derivatives; Step S14: Combine the constraint conditions of the hinge points: , and simplify the above equations by combining them to obtain the single-track dynamics model of a double semi-trailer truck during a right turn; Specifically, in order to further derive the single-track dynamics model of the double semi-trailer truck during a right turn into the state space equation used below, the following dynamic equations are given: ; Step S15: Let the state variables , the observed variables , and 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 ; ; ; Among them, the elements in the matrix are as follows: ; ; ; ; ; ; ; After matrix operations, the state space equation is obtained:

[0026] Among them, is the first-order derivative of the state variable ; ; ; (where \(I_6\) is the 6 - order identity matrix); Step S16: Derive and discretize the space - state equation and the output equation; Since model predictive control (MPC) is a control method based on discrete systems, the space - state equation in continuous time needs to be discretized first. Specifically, the zero - order hold (ZOH) method is used to discretize the space - state 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 using ZOH discretization, the discretized space - state equation and output equation (observation equation) are: ; where represents the discrete - state matrix , input matrix and discrete - time output matrix at the \(k\) - th time step, which are defined as: ; where ; is the integration variable; Furthermore, in this embodiment, the process of establishing the dynamic blind - zone model of the double - semi - trailer truck train in step S2 is specifically as follows: Step S21: First, according to the relevant regulations of "Performance and Installation Requirements for Indirect Vision Devices of Motor Vehicles" and "Dimensions of Driver's Operating Positions for Goods Vehicles", determine the driver's eye - point position: Two points, each offset 32.5 mm horizontally to the left and right at a height of 635 mm above the seat, are used as the driver's eye - points. When the vehicle is moving straight, the range that can be observed through the rear - view 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 rear - view mirror remains unchanged, resulting in some areas that were originally within the "basic visible area" exceeding the visible range of the rear - view mirror. This exceeded part and the projection of the vehicle onto the ground are defined as the blind - zone; Step S22: Obtain the initial data; Obtain the sampling time , the lateral speed of the tractor at the \(k\) - th time step, the longitudinal speed , the yaw angle and yaw - rate , the articulation angle between the tractor and the first semi - trailer, and the angular velocity ​​, the articulation angle between the first semi-trailer and the second semi-trailer and the angular velocity , the total length of the two semi-trailers , ; among them, is the distance from the center of mass of the semi-trailer to the front boundary, is the distance from the center of mass of the semi-trailer to the rear boundary, and the width of the two semi-trailers ; Step S23: After the data preparation is completed, perform dynamic projection prediction of the semi-trailer contour; First, predict the center-of-mass coordinates of the tractor at the time step: ; Subsequently, calculate the coordinates of the tractor saddle. In the tractor coordinate system, the offset of the saddle relative to the center of mass is ([[]] , ), and after rotating and translating it to the global coordinate system, we get: ; 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; 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 ([[]] , ), and after rotating and translating it to the global coordinate system, we get: ; Among them, is the abscissa 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; Given the total length and width of the trailer, define the four-corner offset matrix in the coordinate system of the first semi-trailer as: ; Rotate and translate the four coordinates to get: ; Among them, , and its column vectors correspond to the global coordinates of the four corner points of the first semi-trailer in sequence; ( , ) 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; is the four-corner offset matrix of the first semi-trailer in its coordinate system; 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; In the dynamic projection prediction of the vehicle, the convex hull function (ConvexHull) 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 point sets, which can form a closed shape without any concave parts. In this embodiment, the convex hull is used to calculate the contour projection of the first semi-trailer at the = ({( , ),( , ),( , ),( , )}); Among them, is the contour projection of the first semi-trailer at the is the convex hull calculation function; 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: = ({{( , ),( , ),( , ),( , )}); Among them, is the contour projection of the second semi-trailer at the is the convex hull calculation function; Combine the envelopes (contour projections) of the two semi-trailers: ({ }) Among them, the outline projection of the two - and - a - half - trailer at the th time step; is the convex hull calculation function;

[0027] Step S24: To construct the field - of - view model of the tractor's rearview mirror, realize the dynamic determination of the blind area, and combine the motion state of the vehicle during the turning process, project this field of view onto the ground to obtain the actual observable area (ground visible area) of the rearview mirror.

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

[0029] First, obtain the direction angle of the rearview - mirror mirror surface normal relative to the axis of the tractor coordinate system; the horizontal viewing - angle range of the rearview mirror; the depression angle γ relative to the center horizontal plane of the mirror surface observed by the driver from the rearview mirror, and the local coordinates , , ; 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; Convert it to the global coordinate system to get: ; 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; At the same time, the direction angle of the line - of - sight direction (central axis) of the rearview mirror in the global coordinate is: ; Taking is the vertex of the cone. The rearview mirror's field of view can be regarded as a pyramid, and the direction of its central axis is , the horizontal half-angle is , and the vertical half-angle is γ ; In the horizontal direction, the left and right boundary angles of the pyramid are respectively: ; 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; Starting from the center position of the rearview mirror, the three-dimensional parametric expressions of the two boundary rays in the cone are: ; 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, is a negative value.

[0030] To obtain the farthest visible distance on the ground, the intersection point of the required ray and the ground plane is needed: ; Solve the parameter at the intersection point and project it onto the ground to obtain the farthest visible distance : ; Construct a sector with , as the center and as the radius. Its starting angle and ending angle are respectively and . The formed area is the visible area on the ground observed by the driver through the rearview mirror at time k .

[0031] 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's field-of-view cone onto the ground when the vehicle is moving straight . Then, according to the front and rear boundaries of the semi-trailer truck, it is divided into two sub-areas , . Subsequently, at each time step , according to the two hinge angles and , adjust the positions of the two sub-areas and update them to , ; Specifically, for the sub-region and the coordinates of the i-th point are respectively At each time step according to the changes in the articulation angles and each sub-region will rotate; For the i-th point in the sub-region : ; wherein is the i-th point in the sub-region at time step ; is the articulation angle between the tractor and the first semi-trailer at time step ; is the i-th point in the sub-region in the basic visible region; For the i-th point in the sub-region : ; wherein is the i-th point in the sub-region at time step ; is the articulation angle between the first semi-trailer and the second semi-trailer at time step ; After rotation transformation, the contours of the basic visible region and can be regarded as the set of these transformed points, that is: ; ; wherein is the number of points in ; is the number of points in ; Next, adjust the direction of the rearview mirror vision cone in the global coordinate system according to the yaw angle of the tractor, and calculate the projection of the rearview mirror vision cone on the ground at the k-th time step (ground visible region) ; Finally, is removed from the merged sub-region , and then an union operation is performed with the projection of the semi-trailer body contour to finally obtain the dynamic blind area at the current time step as: .

[0032] Furthermore, in this embodiment, the specific process of obtaining obstacle information in step S3 is as follows: Step S31: Obtain point cloud data and preprocess it to delete out-of-limit points, overly distant points, overly high points, and points on the vehicle itself.

[0033] Step S32: Obtain the blind area in the prediction result output by 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 remaining point cloud after preprocessing, and only retain the points within the ROI range to obtain a more focused point cloud for subsequent object detection and recognition, reducing the computational burden of irrelevant point data.

[0034] 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.

[0035] Step S34: Convert the tensorized point cloud to , , -sized tensors through BatchNorm (batch normalization) and ReLu functions. Then, perform max pooling operations according to the dimension where the Pillar is located, that is, obtain a feature map of , dimensions; in order to obtain pseudo-picture features, convert P to (H, W), and finally obtain , , pseudo-image; where represents the number of feature channels, and H and W are the height and width of the picture 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.

[0036] Step S35: The detection head performs 3D object detection using the SSD method. Specifically, on each feature map output by the Backbone network, a set of default boxes are predefined, which cover target regions 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 class probability. This method enables the model to simultaneously complete object localization and classification in a single forward propagation, greatly improving the detection efficiency. Finally, the preliminary detection results are post-processed by non-maximum suppression (NMS) to filter out duplicate boxes, and only the detection boxes with the highest confidence and less overlap are retained. The final output is the detected object information, including the projected contour of the object on the ground and position.

[0037] 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.

[0038] 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 to optimize the control amount in step S4 is as follows: Step S41: To prevent the sudden change of the system control amount from affecting its continuity, the control increment is used to replace the control amount, and the state quantity deviation and input quantity deviation of the vehicle are constructed: , ; where and are the increments of the state quantity and the control input quantity respectively; , , , , are the reference values of the original corresponding variables at the time step respectively; Substitute into the discretized space state equation in step S16 to obtain the new discretized space state equation and output equation as: ; Construct a new state vector: ; Reconstruct the space state equation and the output equation : ; Among them, , = , = , is the input vector at time k; Step S42: At each sampling moment, predict the vehicle trajectory based on the above-derived space state equation and roll-optimize the control quantity; Specifically, recursively calculate the state sequence of the next steps according to the reconstructed space state equation in Step S41 as the prediction process, and thereby derive the expansion of the system output : ; ;

[0039] ;

[0040] ; According to the above derivation, define step prediction output vector and step input vector as: , ; 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 , so the exponent of the last term is not 0. Combining them, the system prediction output model can be obtained as: ; Among them, , ; 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 less than the predetermined threshold; Specifically, design the objective function in the following form: , , = ; 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; In addition, in order 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 within the entire prediction horizon; Satisfy the control quantity constraint and control increment constraint within the prediction horizon: , , , , ; , , , , ; where 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 step, is the control input increment at the k+i step; Obtain the blind area contour at the step and the obstacle contour , and define the blind area-obstacle safety distance as: ; where is the point in the dynamic blind area , is the point in the obstacle contour ; The safety distance constraint is: ; where is the minimum safety distance between the blind area and the obstacle; Step S44: After the objective function and constraints are established, the MPC trajectory tracking control algorithm based on the augmented state and input increment needs to roll and solve the following constrained QP problem (quadratic equation minimization problem with linear constraints) at each sampling period to obtain the optimal control increment; , , ; ; Step S45: Repeat the above process in the next cycle.

[0041] The present invention also provides an electronic device, including: one or more processors and a memory; wherein, the memory is used to store 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 vision blind area during small-curvature turning of a double semi-trailer truck train.

[0042] 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 vision blind area during small-curvature turning of a double semi-trailer truck train is implemented.

[0043] 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 functions are implemented by a computer executing the program. For example, when the program is stored in the memory of the device and the program in the memory is executed by the processor, all or part of the above functions can be implemented.

[0044] 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.

[0045] 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 should be subject to the protection scope of the claims.

Claims

1. A method for dynamically predicting blind spots and avoiding obstacles in a small curvature turn of a double semi-trailer vehicle train, characterized in that: The method comprises the following steps: Step S1: Obtain the state parameters of the vehicle, establish a single-track dynamics model of a double semi-trailer vehicle train turning right, and obtain a discrete spatial state equation and an output equation after derivation and discretization; Step S2: establishing a dynamic blind spot model of a double semi-trailer vehicle train, and predicting the blind spot through the dynamic blind spot model; wherein the dynamic blind spot model of the double semi-trailer vehicle train is constructed based on the contour projection 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 area of ​​the first and second semi-trailers; Step S3: Expand the blind area predicted in step S2 and set it as an area of ​​interest, use laser radar to detect obstacles, obtain point cloud data, and process the point cloud data in the area of ​​interest to predict obstacle information, where the obstacle information includes obstacle outline and position; Step S4: Based on the spatial state equation and the output equation, the MPC algorithm is used to predict the future vehicle state, and the blind spot predicted by the dynamic blind spot model and the predicted obstacle contour and position are combined to judge the existing risk, and the control amount is optimized in a rolling manner to always keep the safety distance between the blind spot contour and the obstacle not less than the predetermined threshold.

2. The method for dynamic prediction of blind spots and obstacle avoidance of double semi-trailer vehicles in small curvature turns according to claim 1 is characterized in that: The expression of the dynamic blind spot model of the double semi-trailer vehicle train in step S2 is: ； in, Represents the blind spot, 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 silhouette projection of two semi-trailers.

3. The method for dynamic prediction of blind spots and obstacle avoidance of double semi-trailer vehicles in small curvature turns according to claim 1 is characterized in that: The contour projection of the first semi-trailer at time k in step S2 is: = ({( , ),( , ),( , ),( , )})； in, For the The silhouette 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, ( , ) 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 time k: = ({{( , ),( , ),( , ),( , )})； in, For the The silhouette 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, ( , ) The left rear corner of the second semi-trailer, ( , ) is the right rear corner point of the second semi-trailer; The contour projection of the first and second semi-trailers is: ({ })； in, No. The silhouette projection of the two semi-trailers under the time step; Compute function for the convex hull.

4. The method for dynamically predicting blind spots and avoiding obstacles in a small curvature turn of a double semi-trailer vehicle train according to claim 1 is characterized in that: In step S2, the visible area of ​​the ground observed by the driver through the rearview mirror at time k is , is the center of the circle, The sector with radius is the starting angle and ending angle respectively. and The area formed by is the horizontal coordinate 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; The center position of the rearview mirror in the global coordinate system is taken as the vertex of the cone, and the field of view of the rearview mirror is regarded as a pyramid with the central axis direction as , the horizontal half angle is , the vertical half angle is γ ; and is the left and right boundary angles of the pyramid in the horizontal direction, and the expression is: ; ; ； in, The maximum visible distance.

5. The method for dynamic prediction of blind spots and obstacle avoidance of double semi-trailer vehicles in small curvature turns according to claim 1 is characterized in that: The method for determining the basic visible area of ​​the first and second semi-trailers at time k in step S2 is: determine the basic visible area when the vehicle is traveling in a straight line 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 , , then at each time step Next, 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 method for dynamic prediction of blind spots and obstacle avoidance of double semi-trailer vehicles in small curvature turns according to claim 1 is characterized in that: Step S3 specifically includes the following steps: Step S31: Acquire point cloud data and pre-process it; Step S32: According to the blind area predicted in step S2, expand it outward by 1 meter according to its contour shape and set it as the area of ​​interest, and only retain the point cloud within the area of ​​interest to obtain a more focused point cloud; Step S33: Divide the point cloud into multiple uniform small grids, each grid is a Pillar, and encode it as , , ;in, Represents the number of selected Pillars, It 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 , , After that, the maximum pooling operation is performed according to the dimension where Pillar is located, and the obtained , Dimensional feature map; transform P into (H, W), and finally obtain , , A pseudo image of Represents the number of feature channels, H and W are the height and width of the image respectively; then, the pseudo image is input into the 2D convolutional network for feature extraction; finally, the 2D convolutional network will output feature maps of different scales; Step S35: Use the SSD method to perform 3D target detection; finally, post-process the preliminary detection results through non-maximum suppression to output the detected object information, including the projection contour of the object on the ground and location.

7. The method for dynamic prediction of blind spots and obstacle avoidance of double semi-trailer vehicles in small curvature turns according to claim 1, characterized in that: In step S4, the vehicle state deviation and input deviation are first constructed, and the discrete spatial state equation in step S1 is updated to obtain a new discrete spatial state equation and output equation; then a new state vector is constructed to reconstruct the reconstructed spatial state equation. And the output equation : ； in, , = , = , is the input vector at time k; the discrete state matrix is , the input matrix is and the discrete-time output matrix is , the new state vector is ; Then, the future is calculated recursively according to the reconstructed space state equation The state sequence of the step.

8. The method for dynamically predicting blind spots and avoiding obstacles in a small curvature turn of a double semi-trailer vehicle train according to claim 3 is characterized in that: Section 1 The four-corner coordinates of the semi-trailer at time k The calculation method is: ； ； ； The rotation matrix is , 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 semitrailer in the global coordinate system; is the heading angle of the first semi-trailer, ; is the distance from the center of mass of the first semitrailer to the front boundary, is the distance from the center of mass of the first semitrailer to the rear boundary, is the width of the first semi-trailer; is the yaw angle of the tractor, It is the articulation angle between the tractor and the first semi-trailer.

9. The method for dynamic prediction of blind spots and obstacle avoidance of double semi-trailer vehicles in small curvature turns 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 limited by the input time step, so the final expression of the predicted output model is: ； in, , , represents the control input increment, the new state vector , and are the increments of state quantity and control input quantity respectively, is the number of steps to predict the output vector, is the number of input vector steps.

10. The method for dynamic prediction of blind spots and obstacle avoidance of double semi-trailer vehicles in small curvature turns according to claim 9, characterized in that: In step S4, the MPC algorithm solves the constrained QP problem in a rolling manner in each sampling period to obtain the optimal control increment; , , ； ； in, , , = ; is the predicted output value of the system; is the output reference value; Control increments for the system; and are the weighting matrices of system output and control increment respectively; is the penalty weight for the relaxation amount, 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; Dynamic blind zone The point in The obstacle outline The point in Blind spot – minimum safe distance to obstacles, is the control input of the k+ith step; It is the k+i-th step blind spot – obstacle safety distance.

Images