CN113721606B

CN113721606B - Following type automatic driving logistics vehicle control system and method

Info

Publication number: CN113721606B
Application number: CN202110936726.6A
Authority: CN
Inventors: 黄晋; 贾一帆; 钟志华
Original assignee: Tsinghua University
Current assignee: Tsinghua University
Priority date: 2021-08-16
Filing date: 2021-08-16
Publication date: 2023-04-25
Anticipated expiration: 2041-08-16
Also published as: CN113721606A

Abstract

The application discloses following formula autopilot commodity circulation vehicle control system and method, this system by manual driving guide car and autopilot follow car and constitute, adopt flexible physical connection and wireless connection mixed formation to drive networking scheme, realize closely formation and go. The manual driving mode of the guided vehicle is adopted, the guided vehicle simulates the driving behavior of the guided vehicle in real time, and the path following control is carried out according to the relative track reconstruction, so that the automatic driving and formation driving technology based on a sensing system and a high-precision satellite positioning system is not completely relied on, the reliability of the whole scheme is superior to that of the existing formation driving mode based on satellite positioning and pure wireless communication, and the complexity is greatly reduced. Through the technical scheme in this application, with the operation scene dimension reduction of high autopilot truck for having the supervision formula to follow autopilot scene, realize a novel guide-autopilot train system of low cost, satisfy logistics industry wisdom transformation demand more.

Description

Following type automatic driving logistics vehicle control system and method

Technical Field

The application relates to the technical field of automatic driving, in particular to a following type automatic driving logistics vehicle control system and a following type automatic driving logistics vehicle control method.

Background

With the development of regional economy and the continuous improvement of highway infrastructure and vehicles, the demand for medium-long distance highway transportation increases, and highway freight is moving toward rapid, long distance and heavy load. The large-tonnage heavy special transport vehicle becomes the main force of the future road transport vehicle in China because of high-speed safety and low unit transport cost. The special vehicle products are developed to heavy duty, strong special functions and high technical content. On the one hand, however, due to resource and energy limitations, the road network mileage and the freight vehicle holding capacity cannot be increased by equal factors; on the other hand, the traditional highway freight logistics is in shortage of drivers, high in cost and unable to adapt to new requirements because of low efficiency, high labor intensity and severe working environment. The existing highway cargo transportation presents the problems of blockage, deficiency, danger and poor, so the rapid increase of the turnover of the highway cargo brings out new requirements on rapidness, accuracy and safety for freight logistics.

The intelligent transportation system can improve the road traffic safety level, reduce traffic jam, improve the traffic capacity of a road network, reduce the pollution of automobile transportation to the environment, and improve the automobile transportation productivity and economic benefit. With the development of intelligent transportation system technology, high technologies such as electronic technology, information technology, communication technology and system engineering are widely applied in the field of road transportation, and logistics transportation information management, transportation tool control technology, transportation safety technology and the like are all greatly leaped, so that the traffic capacity of a road network is greatly improved.

However, existing automatic driving methods for formation based on satellite positioning and pure wireless communication rely on a large number of high-cost sensors, such as RTK positioning devices, lidar, vision sensors, etc., and require complex automatic driving algorithms to hold;

in addition, because the logistics vehicles have the problems of large volume, large inertia, difficult steering and the like, and the road traffic conditions are complex and changeable, the sensing system and the control system of the automatic driving vehicles at the present stage are not adequate, and the problem of high cost exists, which is contrary to the concept that the intelligent transportation system improves the transportation productivity and the economic benefit of the automobiles. Therefore, the realization of the full automatic driving of the logistics vehicles at the present stage is not practical at the technical level, the safety level or the regulation level.

Disclosure of Invention

The purpose of the present application is: at least one of the problems in the existing logistics vehicle formation automatic driving based on satellite positioning and pure wireless communication is solved.

The technical scheme of the first aspect of the application is that: there is provided a following type automatic driving logistics vehicle control system, the system being adapted for automatic driving control of a following vehicle in a logistics vehicle consist, the logistics vehicle consist comprising a lead vehicle and at least one following vehicle, the system comprising: the system comprises a vehicle-vehicle relative positioning system, a vehicle state information acquisition system, a following vehicle planning and control system and a following vehicle control execution system; the vehicle-vehicle relative positioning system is used for collecting vehicle-vehicle relative positioning information; the vehicle state information acquisition system is used for acquiring vehicle state information of the guide vehicle and the following vehicle; the following vehicle planning and control system is used for calculating the relative pose of the guided vehicle in a following vehicle coordinate system according to the vehicle relative positioning information and the vehicle state information, determining a following vehicle reference path of the following vehicle through a fitting algorithm, and determining following running parameters of the following vehicle according to the following vehicle reference path and the vehicle state information; the following vehicle control execution system is used for controlling the following vehicle to automatically follow driving according to following driving parameters, wherein the following driving parameters at least comprise the expected pedal opening degree of the following vehicle and the expected steering column angle of the following vehicle.

In any one of the above technical solutions, further, the following vehicle planning and control system includes a relative pose calculation module of the guiding vehicle, the relative pose calculation module of the guiding vehicle is used for calculating the relative pose of the guiding vehicle, and the calculation process of the relative pose of the guiding vehicle specifically includes: respectively according to steering column angle delta of the guiding vehicle in the vehicle state information _s1 Steering column angle delta of follower _s2 Longitudinal speed v of the guided vehicle _x1 Longitudinal speed v of following vehicle _x2 Calculating yaw rate omega of guided vehicle ₁ Yaw rate omega of following vehicle ₂ The method comprises the steps of carrying out a first treatment on the surface of the According to yaw rate omega of the guided vehicle ₁ Yaw rate omega of following vehicle ₂ Calculating the relative pose estimation value of the guided vehicle, and according to the relative pose estimation value of the guided vehicle

Relative pose measurement q of guide vehicle for UWB positioning system _U Relative guide vehicle pose q of vehicle-vehicle connecting mechanism _G And performing fusion calculation to generate the relative pose q of the guide vehicle.

In any one of the above technical solutions, further, the following vehicle planning and control system further includes a following vehicle reference path construction module, where the following vehicle reference path construction module is configured to determine a following vehicle reference path of the following vehicle, and specifically includes: according to the movement and rotation quantity of the following vehicle coordinate system, updating and advancing the coordinates of the relative path of the guiding vehicle in the previous sampling period; taking the relative pose of the guide vehicle in the current sampling period as an initial point of the relative path of the guide vehicle in the current sampling period; and fitting the relative paths of the guided vehicles by using a fitting algorithm, and determining the reference paths of the following vehicles.

In any of the above solutions, further, the following vehicle planning and control system further includes: the system comprises a following vehicle longitudinal control module, a following vehicle transverse instability judging module and a following vehicle transverse control module; the following vehicle longitudinal control module is used for constructing a target equation x according to a following vehicle reference path _1RP (a) =0, and calculate the target equationNumerical solution a _y2 According to the numerical solution a _y2 The following vehicle reference path calculates the opening degree of an expected pedal of the following vehicle; the following vehicle transverse instability judging module is used for solving a according to the numerical value _y2 And a following vehicle reference path, calculating a following vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 And according to the following vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 Calculating the track following transverse instability degree D _us The method comprises the steps of carrying out a first treatment on the surface of the The transverse control module of the following vehicle is used for controlling the transverse deviation e of the following vehicle _y2 And degree of track following lateral instability D _us Determining a desired steering column angle for a follower

In any of the above technical solutions, further, the vehicle-to-vehicle relative positioning system further includes: a vehicle-to-vehicle connection mechanism; the guiding vehicle is connected with the following vehicles and the two adjacent following vehicles through a vehicle-vehicle connecting mechanism, and the vehicle-vehicle connecting mechanism is used for wired data transmission.

The technical scheme of the second aspect of the application is that: the following type automatic driving logistics vehicle control method is suitable for automatic driving control of following vehicles in a logistics vehicle queue, wherein the logistics vehicle queue comprises a guide vehicle and at least one following vehicle, and the method comprises the following steps of: step 1, calculating the relative pose of a guided vehicle in a following vehicle coordinate system according to acquired vehicle-to-vehicle relative positioning information and vehicle state information; step 2, taking the relative pose of the guided vehicle as an initial point of a guided vehicle relative path of a current sampling period, updating the guided vehicle relative path of the current sampling period according to the guided vehicle relative path of a previous sampling period and the movement and rotation quantity of a following vehicle coordinate system of the current sampling period, and determining a following vehicle reference path of the following vehicle through a fitting algorithm, wherein the movement and rotation quantity of the following vehicle coordinate system is determined by the following vehicle speed and the following vehicle yaw rate of the following vehicle in the current sampling period; and 3, determining following running parameters of the following vehicle according to the following vehicle reference path and the vehicle state information, wherein the following running parameters at least comprise the expected pedal opening degree of the following vehicle and the expected steering column angle of the following vehicle.

In any one of the above technical solutions, further, the guiding vehicle is connected with the following vehicle and the following vehicle is connected with the preceding following vehicle through a vehicle-vehicle connection mechanism, UWB positioning systems are arranged on the guiding vehicle and the following vehicle, and in step 1, the relative pose q of the guiding vehicle is calculated, which specifically includes:

step 11, respectively according to the steering column angle delta of the guided vehicle in the vehicle state information _s1 Steering column angle delta of follower _s2 Longitudinal speed v of the guided vehicle _x1 Longitudinal speed v of following vehicle _x2 Calculating yaw rate omega of guided vehicle ₁ Yaw rate omega of following vehicle ₂ ；

Step 12, according to the yaw rate omega of the guided vehicle ₁ Yaw rate omega of following vehicle ₂ Calculating the relative pose estimation value of the guided vehicle, and according to the relative pose estimation value of the guided vehicle

In any of the above technical solutions, further, calculating a yaw rate ω of the following vehicle ₂ Specifically comprises the following steps:

according to the steering column angle delta of the following vehicle _s2 The rotation angle delta of the front wheel of the following car is obtained through conversion _w2 In combination with the longitudinal speed v of the following vehicle _x2 Calculating a yaw rate calculation value omega of the following vehicle _{2_KIN} ；

Acquiring a value omega according to the yaw rate of the following vehicle in the vehicle state information _{2_IMU} And a following vehicle yaw rate calculation value omega _{2_KIN} Calculating yaw rate omega of follower vehicle ₂ Yaw rate ω of follower ₂ The calculation formula of (2) is as follows:

ω ₂ ＝(1-K _yaw )ω _{2_KIN} +K _yaw ω _{2_IMU}

wherein K is _yaw To blend the proportionality coefficients for yaw rate, v _low And v _high The lower limit vehicle speed and the upper limit vehicle speed are respectively the yaw rate fusion.

In any of the above solutions, further, in step 3, the method for determining the desired pedal opening of the following vehicle specifically includes:

step 31, constructing a target equation x according to the following vehicle reference path _1RP (a) =0, and calculates a numerical solution a of the objective equation _y2 According to the numerical solution a _y2 And a following vehicle reference path, calculating the longitudinal track distance d between the guiding vehicle and the following vehicle _rel The corresponding calculation formula is:

step 32, according to the longitudinal track distance d between the guided vehicle and the following vehicle _rel The expected pedal opening of the following vehicle is calculated, and the corresponding calculation formula is as follows:

wherein K is _ff As a feed-forward scaling factor,

for a pedal feed-forward follower a pedal opening is desired, < >>

For kinematic feedback of the desired pedal opening of the following vehicle, K _a Longitudinal acceleration feedback coefficient, K _v Vehicle speedFeedback coefficient, K _d Longitudinal trajectory distance feedback coefficient, a _x1 To guide the longitudinal acceleration of the vehicle, a _x2 To follow the longitudinal acceleration of the vehicle v _x1 To guide the longitudinal speed of the vehicle v _x2 To follow the longitudinal speed of the vehicle, d _ref For guiding the longitudinal track distance reference value of the car and the following car.

In any of the above solutions, further, in step 3, the method for determining the desired steering column angle of the follower vehicle specifically includes:

step 33, solving a according to the numerical value _y2 And a following vehicle reference path, calculating a following vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 ；

Step 34, according to the following vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 Calculating the track following transverse instability degree D _us The corresponding calculation formula is:

D _us ＝D _{us_y2} (e _y2 )+D _{us_θ2} (e _θ2 )

wherein D is _{us_y2} (e _y2 ) And D _{us_θ2} (e _θ2 ) E is the degree of lateral deviation instability and the degree of orientation deviation instability respectively _{y2_eff} 、e _{θ2_eff} The transverse effective deviation and the direction effective deviation of the following vehicle are respectively; k (K) _ey 、K _eθ The lateral deviation instability growth coefficient and the orientation deviation instability growth coefficient are respectively;

step 35, according to the following vehicle lateral deviation e _y2 And degree of track following lateral instability D _us Determining a desired steering column angle for a follower

The corresponding calculation formula is:

wherein K is _s As a coefficient of the steering proportion,

for following the desired front wheel angle of the vehicle +.>

For the stable state following the expected front wheel turning angle of the vehicle, the unstable state following the expected front wheel turning angle +. >

For the lateral deviation feedback portion,

pretarget part of the corner for the relative path, K _{P_ey} 、K _{I_ey} 、K _{D_ey} The proportional coefficient, the integral coefficient and the differential coefficient of the lateral deviation feedback are respectively.

The beneficial effects of this application are:

the technical scheme in this application comprises manual driving guide car and autopilot follower, adopts flexible physical connection and wireless connection mixed formation to drive networking scheme, realizes closely formation and goes. The manual driving mode of the guided vehicle is adopted, the guided vehicle simulates the driving behavior of the guided vehicle in real time, and the path following control is carried out according to the relative track reconstruction, so that the automatic driving and formation driving technology based on a sensing system and a high-precision satellite positioning system is not completely relied on, the reliability of the whole scheme is superior to that of the existing formation driving mode based on satellite positioning and pure wireless communication, and the complexity is greatly reduced. The operation scene of the high-speed automatic driving truck is reduced in dimension to be a supervised type following automatic driving scene, so that the novel low-cost guiding-following automatic driving train system is realized, and the intelligent transformation requirement of the logistics industry is met. The significance of the method is as follows:

1. solving the shortages and high cost of freight logistics drivers: the guiding vehicle drives manually, and the following vehicle drives automatically, so that half drivers are reduced, and driving intensity and labor cost of the drivers are further reduced.

2. Promote highway freight transportation commodity circulation transfer efficiency: the driver of the guided vehicle and the safety personnel of the following vehicle alternately drive, so that the driving efficiency is improved; the dispatching system dispatches the formed driving vehicles integrally, so that the overall efficiency is improved; the queue driving is beneficial to reducing wind resistance and oil consumption.

3. The long-distance driving safety of freight drivers is enhanced: the vehicle state is monitored in real time in the whole process, and the reliability of the automatic driving system of the following vehicle is higher than that of a human driver.

Drawings

The advantages of the foregoing and/or additional aspects of the present application will become apparent and readily appreciated from the description of the embodiments, taken in conjunction with the accompanying drawings, wherein:

FIG. 1 is a schematic illustration of a trailing autopilot logistics vehicle in accordance with one embodiment of the present application;

FIG. 2 is a schematic block diagram of a follow-up autopilot logistics vehicle control system in accordance with one embodiment of the present application;

FIG. 3 is a schematic illustration of UWB positioning system acquisition parameters according to one embodiment of the present application;

FIG. 4 is a schematic diagram of yaw rate relationship in a kinematic model of a vehicle according to one embodiment of the present application;

FIG. 5 is a relative pose schematic of a lead vehicle according to an embodiment of the present application;

FIG. 6 is a relative path reconstruction schematic of a lead vehicle according to one embodiment of the present application;

FIG. 7 is a diagram of a guided vehicle relative path coordinate update according to one embodiment of the present application;

FIG. 8 is a schematic block diagram of a follower longitudinal motion control algorithm according to one embodiment of the present application;

FIG. 9 is a schematic block diagram of a following lateral motion control algorithm according to one embodiment of the present application;

fig. 10 is a schematic diagram of a follower lateral motion control according to one embodiment of the present application.

Detailed Description

In order that the above-recited objects, features and advantages of the present application will be more clearly understood, a more particular description of the application will be rendered by reference to the appended drawings and appended detailed description. It should be noted that, without conflict, the embodiments of the present application and features of the embodiments may be combined with each other.

In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present application, however, the present application may be practiced otherwise than as described herein, and thus the scope of the present application is not limited to the specific embodiments disclosed below.

Embodiment one:

as shown in fig. 1 and 2, the present embodiment provides a following type automatic driving logistics vehicle control system 100, the system 100 is suitable for automatic driving control (automatic following) of following vehicles in a logistics vehicle array, the logistics vehicle array includes a guiding vehicle 1 and at least one following vehicle 2, the following vehicles 2 are sequentially connected end to end with the guiding vehicle 1 or the previous following vehicle 2 through a vehicle-to-vehicle connection mechanism 33, wherein the guiding vehicle 1 is driven by a human driver, and the following vehicles 2 automatically follow under the control of the control system. The system 100 includes: the system comprises a vehicle-to-vehicle positioning system 3, a vehicle state information acquisition system 4, a following vehicle planning and control system 5 and a following vehicle control execution system 6.

In the present embodiment, the relative positioning system 3 includes a UWB positioning system 31, a device 32 for measuring the angle between the head and the container of the guided vehicle, and a vehicle-to-vehicle connection mechanism 33. The vehicle-to-vehicle relative positioning system 3 is used for collecting vehicle-to-vehicle relative positioning information.

The UWB positioning system 31 is an ultra wideband positioning system, and is disposed above the tail of the container of the guiding vehicle 1 and above the head of the following vehicle 2, so as to realize the relative positioning function of the vehicle workshop. The relative positioning information of the vehicle collected by the UWB positioning system 31 includes: relative distance d of UWB positioning system _U Azimuth angle theta of UWB positioning system _UO And UWB positioning system attitude angle theta _UA 。

It should be noted that, when the logistics vehicle queue includes a plurality of following vehicles, the first logistics vehicle is a guiding vehicle, the second to last logistics vehicles are the following vehicles in the queue, and the following control of the third logistics vehicle is determined according to the vehicle relative positioning information and the vehicle state information of the second logistics vehicle and the third logistics vehicle, at this time, the second logistics vehicle is equivalent to the guiding vehicle of the third logistics vehicle.

For ease of understanding, the present embodiment is set to include only one lead vehicle and one following vehicle.

Specifically, as shown in fig. 3, UWB positioning units on the left side and the right side of the tail of a container of a guided vehicle are respectively (1) and (2), and UWB positioning units on the head of a following vehicle are respectively (3) and (4); UWB positioning system relative distance d _U The length of the connecting line between the midpoint of (1) and (2) and the midpoint of (3) and (4) is the azimuth angle theta of the UWB positioning system _UO The attitude angle theta of the UWB positioning system is the included angle between the midpoint of the (1), the midpoint of the (2), the midpoint of the (3), the midpoint of the (4), the midpoint of the (3) and the perpendicular bisector of the (4) _UA Is the included angle between the perpendicular bisectors (1), (2) and (3) and (4).

The guide vehicle head and container included angle measuring device 32 is used for collecting the horizontal included angle theta of the longitudinal axis of the guide vehicle and the following vehicle _1P 。

The guiding vehicle 1 is connected with the following vehicle 2 and two adjacent following vehicles through the vehicle-vehicle connecting mechanism 33, the vehicle-vehicle connecting mechanism 33 can realize wired data transmission between vehicles, and the vehicle-vehicle connecting mechanism 33 is connected with the vehicleThe horizontal included angle of the longitudinal axis connecting the vehicles comprises the horizontal angle theta of the vehicle communication connecting line (vehicle connecting mechanism 33) and the central axis of the guiding vehicle container _G1 Horizontal angle theta between vehicle-to-vehicle connection mechanism 33 and central axis of following vehicle head _G2 Is provided. The information collected by the vehicle-to-vehicle connection mechanism 33 includes: length d of vehicle-to-vehicle communication connection line _G Horizontal angle theta of vehicle-to-vehicle communication connecting line and central axis of guide vehicle container _G1 And the horizontal angle theta between the vehicle head central axis and the following vehicle _G2 ；

In the present embodiment, the vehicle state information acquisition system 4 is used for acquiring vehicle state information of the lead vehicle and the following vehicle, and the vehicle state information acquisition system 4 includes an accelerator and brake pedal opening sensor 41, a steering column angle sensor 42, and a kinematic information combination sensor 43.

The vehicle-to-vehicle connection mechanism 33 collects the vehicle-to-vehicle communication connection line length d _G Horizontal angle theta of vehicle-to-vehicle communication connecting line and central axis of guide vehicle container _G1 And the horizontal angle theta between the vehicle head central axis and the following vehicle _G2 ；

The accelerator and brake pedal opening sensor 41 collects the accelerator pedal opening p of the guided vehicle _acc1 Brake pedal opening p of guided vehicle _brk1 Accelerator pedal opening p of following vehicle _acc2 Brake pedal opening p of following vehicle _brk2 ；

Steering column angle sensor 42 collects a lead vehicle steering column angle delta _s1 Steering column angle delta of follower _s2 ；

The kinematic information combination sensor 43 acquires the longitudinal acceleration a of the guided vehicle _{x1_IMU} Yaw rate acquisition value omega of guide vehicle _{1_IMU} Longitudinal acceleration a of following vehicle _{x2_IMU} Yaw rate acquisition value omega of following vehicle _{2_IMU} ；

The vehicle state information acquisition system 4 is also capable of reading the longitudinal vehicle speed v of the lead vehicle directly from the CAN bus of the lead vehicle 1 and the following vehicle 2 respectively _x1 Longitudinal speed v of following vehicle _x2 。

The information acquisition method in this embodiment is not limited to this embodiment.

The system also comprises: a follower planning and control system 5. The following vehicle planning and control system 5 is used for calculating the relative pose of the guided vehicle in a following vehicle coordinate system according to the vehicle relative positioning information and the vehicle state information, determining a following vehicle reference path of the following vehicle through a fitting algorithm, and determining following running parameters of the following vehicle according to the following vehicle reference path and the vehicle state information;

Further, the following vehicle planning and control system 5 includes a guided vehicle relative pose calculating module 51, where the guided vehicle relative pose calculating module 51 is configured to calculate a relative pose of the guided vehicle, and a process of calculating the relative pose of the guided vehicle specifically includes:

respectively according to steering column angle delta of the guiding vehicle in the vehicle state information _s1 Steering column angle delta of follower _s2 Longitudinal speed v of the guided vehicle _x1 Longitudinal speed v of following vehicle _x2 Calculating yaw rate omega of guided vehicle ₁ Yaw rate omega of following vehicle ₂ ；

The yaw rate ω of the lead vehicle ₁ Yaw rate omega of following vehicle ₂ The calculation manner of (2) is the same, therefore, the present embodiment uses the following vehicle yaw rate ω ₂ An example is described.

In the present embodiment, the following vehicle yaw rate ω is calculated ₂ The method of (1) specifically comprises the following steps:

first, according to the steering column angle delta of the following vehicle _s2 The rotation angle delta of the front wheel of the following car is obtained through conversion _w2 In combination with the longitudinal speed v of the following vehicle _x2 Calculating a yaw rate calculation value omega of the following vehicle _{2_KIN} 。

As shown in fig. 4, in the two-degree-of-freedom vehicle kinematic model, the following vehicle front wheel rotation angle delta can be calculated _w2 Longitudinal speed v of following vehicle _x2 Deriving a calculated yaw rate omega of the following vehicle _{2_KIN} The formula is as follows:

ω _{2_KIN} ＝v _x2 tanδ _w2 /l ₂

Wherein, I ₂ For following vehicles and guidingWheelbase, delta of head part between vehicles _w2 To follow the front wheel angle, v _x2 Is the longitudinal speed of the following vehicle, wherein the rotation angle delta of the front wheel of the following vehicle _w2 Steering column angle delta of follower _s2 The conversion relation is as follows:

δ _w2 ＝K _s δ _s2

wherein K is _s Is the steering scaling factor.

Next, a following vehicle yaw rate acquisition value ω is acquired from the vehicle state information _{2_IMU} And a following vehicle yaw rate calculation value omega _{2_KIN} Calculating yaw rate omega of follower vehicle ₂ Yaw rate ω of follower ₂ The calculation formula of (2) is as follows:

ω ₂ ＝(1-K _yaw )ω _{2_KIN} +K _yaw ω _{2_IMU}

In this embodiment, the following vehicle yaw rate acquisition value ω _{2_IMU} The sensor can directly measure the data, such as a motion combination sensor, and the data is accurate but contains larger noise; following vehicle yaw rate calculation value omega calculated by vehicle kinematics model _{2_KIN} The noise is small, but a certain error exists under the influence of the clearance of the steering transmission mechanism, the tire slip angle and the like. Therefore, in order to improve the accuracy of the calculation of the yaw rate of the following vehicle, a weighting algorithm is adopted to acquire a value omega of the yaw rate of the following vehicle _{2_IMU} Calculated value ω of yaw rate of following vehicle _{2_KIN} And performing weighting operation.

Thus, according to the following vehicle longitudinal speed v _x2 Determining the weight values of the two, and enlarging omega in the low-speed stage of the vehicle with more accurate kinematic model _{2_KIN} Is at the sensorVehicle high speed stage increasing omega with less noise effect _{2_IMU} Is a weight of (a). When v _x2 <v _low Or v _x2 ≥v _high When the data are not fused, omega is directly adopted _{2_KIN} Or omega _{2_IMU} As the final yaw rate ω ₂ The method ensures the authenticity of the yaw rate of the following vehicle, and further improves the accuracy of following control of the following vehicle.

According to yaw rate omega of the guided vehicle ₁ Yaw rate omega of following vehicle ₂ Calculating the relative pose estimation value of the guided vehicle, and according to the relative pose estimation value of the guided vehicle

Relative pose measurement q of guide vehicle for UWB positioning system _U Relative guide vehicle pose q of vehicle-vehicle connecting mechanism _G Performing fusion calculation to generate a relative pose q of the guide vehicle;

as shown in fig. 5 (a), a guide car coordinate system x is first defined ₁ O ₁ y ₁ And the following car coordinate system and x ₂ O ₂ y ₂ . Guide car coordinate system x ₁ O ₁ y ₁ Is fixedly connected with the head of the guiding vehicle, wherein the origin of coordinates O ₁ Is positioned at the midpoint of the rear axle of the head of the guiding vehicle, x ₁ The shaft is forwards along the longitudinal axis of the head of the guide car, y ₁ The shaft points to the left of the longitudinal axis of the head of the guiding vehicle; following vehicle coordinate system x ₂ O ₂ y ₂ Is fixedly connected with the head of the following vehicle, wherein the origin of coordinates O ₂ Is positioned at the midpoint of the rear axle of the head of the following vehicle, x ₂ The shaft moves forward along the longitudinal axis of the head of the following car, y ₂ The axle is directed to the left of the longitudinal axis of the following locomotive.

In this embodiment, the relative pose of the guided vehicle is defined in the following vehicle coordinate system x ₂ O ₂ y ₂ In the guide vehicle coordinate system origin O ₁ Transverse and longitudinal of (2) coordinate x _1R 、y _1R And x ₁ Axis and x ₂ Included angle theta of axes _1R The method comprises the steps of carrying out a first treatment on the surface of the The relative pose of the guided vehicle is represented by vector q, i.e. q= (x) _1R ,y _1R ,θ _1R ) ^T 。

The relative position and orientation angle of the guide vehicle head relative to the plane of the following vehicle head can be described, and the guide vehicle head can be used for reconstructing the subsequent guide vehicle relative path, but in order to reduce the realization cost of automatic driving of the logistics vehicle, an absolute position sensor is not present in the embodiment, so the relative position and orientation of the guide vehicle cannot be directly obtained.

In addition, since the guide vehicle consists of a vehicle head and a container, the relative pose of the guide vehicle cannot be obtained by a single sensor, and the measured values of a plurality of sensors are needed to form a measuring chain for calculation.

In order to reduce accumulated errors caused by measurement of a plurality of sensors, the embodiment obtains the relative pose measurement value q of the guided vehicle based on the UWB system by arranging two positioning systems _U And guided vehicle relative pose measurement q based on vehicle-to-vehicle connection mechanism _G And according to the estimated value variation of the relative pose of the guide vehicle

And (5) performing fusion calculation.

Specifically, in the sampling period Δt, displacement estimated values of the lead vehicle and the following vehicle in respective coordinate systems are calculated and obtained respectively:

the specific formula is as follows:

calculating and obtaining the estimated value variation of the relative pose of the guided vehicle in the sampling period delta T

The specific formula is as follows:

in the method, in the process of the invention,

respectively the abscissa x and the ordinate x of the guided vehicle in the current sampling period _1R 、y _1R And an included angle theta _1R The variation of (x), x' _1R 、y′ _1R 、θ′ _1R Respectively the X-axis and X-axis of the guided vehicle in the previous sampling period _1R 、y _1R And an included angle theta _1R Is a value of (a).

Thus, an estimated value of the relative pose of the guided vehicle is calculated

The specific formula is as follows:

wherein q '= (x' _1R ,y′ _1R ,θ′ _1R ) ^T The relative pose of the guide vehicle is the previous sampling period.

Referring to fig. 5 (b), relative pose measurement q of guided vehicle based on UWB positioning system _U ＝(x _U1R ,y _U1R ,θ _U1R ) ^T The calculation formula of (2) is as follows:

wherein, I _2UO Is from the midpoint of the UWB positioning system (3) and (4) to the origin O of the following vehicle coordinate system ₂ Is a horizontal distance of (2); l (L) _1PU To guide the rotation shaft of the traction seat of the vehicle to the UWB positioning system (1) and (2)Horizontal distance of the midpoint of the number; l (L) _1PO For the rotation shaft of the traction seat of the guide vehicle to the origin O of the coordinate system of the guide vehicle ₁ Is a horizontal distance of (c).

According to fig. 5 (c), the relative pose q of the lead car based on the car-to-car connection mechanism _G ＝(x _G1R ,y _G1R ,θ _G1R ) ^T The calculation formula of (2) is as follows:

wherein, I _2GO For connecting the vehicle-vehicle connecting mechanism and the head of the following vehicle to the origin O of the following vehicle coordinate system ₂ Is a horizontal distance of (2); l (L) _1PG The horizontal distance from the rotating shaft of the traction seat of the guiding vehicle to the connecting point of the vehicle connecting mechanism and the tail of the guiding vehicle is the same.

When fusion calculation is carried out, the calculation formula of the relative pose q of the guide vehicle is as follows:

q＝K _q q _U +(1-K _q )q _G

wherein K is _q In order to guide the relative pose fusion proportionality coefficient of the vehicle, the calculation formula is as follows:

K _q ＝(K _qx +K _qy +K _qθ )/3

that is, in the present embodiment, when fusion calculation of the relative pose of the guided vehicle is performed, the relative pose measurement q of the guided vehicle is measured according to the UWB positioning system _U Relative guide vehicle pose q of vehicle-vehicle connecting mechanism _G Pose estimation value relative to guide vehicle

The smaller the deviation, the larger the ratio in fusion, the closer the calculated relative pose q of the guided vehicle to the true value.

In this embodiment, the following vehicle planning and control system 5 further includes a following vehicle reference path building module 52, where the following vehicle reference path building module 52 is configured to determine a following vehicle reference path of the following vehicle, and specifically includes: according to the movement and rotation quantity of the following vehicle coordinate system, updating and advancing the coordinates of the relative path of the guiding vehicle in the previous sampling period; taking the relative pose of the guide vehicle in the current sampling period as an initial point of the relative path of the guide vehicle in the current sampling period; and fitting the relative paths of the guided vehicles by using a fitting algorithm, and determining the reference paths of the following vehicles.

Specifically, the following vehicle reference path planning is based on the relative path reconstruction of the guided vehicle. As shown in fig. 6, the guided vehicle relative path { a } _i I=0, 1,2,..n+1 } is referred to in the current following car coordinate system x ₂ O ₂ y ₂ Guiding a set of historical relative poses of the vehicle; because the sampling exists at time intervals, the actually collected relative path of the guide vehicle consists of discrete sampling points, A is used for _i i=0, 1, 2..represents, a _i ＝(x _1Ri ,y _1Ri ,θ _1Ri ) ^T Contains three elements, namely, the origin O of the coordinate system of the guided vehicle at the sampling time ₁ In the current following car coordinate system x ₂ O ₂ y ₂ Middle abscissa x _1Ri 、y _1Ri And sampling time x ₁ Axis and current time x ₂ Included angle theta of axes _1Ri 。

Let the sampling period be DeltaT, A ₀ ＝(x _1R ,y _1R ,θ _1R ) ^T The sampling point at the current moment is equivalent to the relative pose q of the guiding vehicle at the current moment; a is that _i ＝(x _1Ri ,y _1Ri ,θ _1Ri ) ^T The sampling point which is i delta T and is before i times of sampling is in the current following car coordinate system x ₂ O ₂ y ₂ Is a pose of the model (a).

A _n ＝(x _1Rn ,y _1Rn ,θ _1Rn ) ^T For nDeltaT, i.e. the sampling point preceding n samplings, in the current following vehicle coordinate system x ₂ O ₂ y ₂ The pose of (3); wherein n is determined by: a is that _n Is positioned at y ₂ Right side of axis and distance y ₂ The closest sampling point of the axis, i.e. at x _1Ri >X in sample point of 0 _1Ri The smallest one.

Thus, in determining the follower reference path of the follower, the guided vehicle relative path of the last sampling period is the measured value.

If present, as shown in FIG. 6 (a), at y ₂ Sampling points to the left of the axis, i.e. where x is present _1Ri <Sample point of 0, A _n Is determined to be located at y ₂ Right side of axis and distance y ₂ The nearest sampling point of the axis, A _n+1 Is A _n The previous sample point, i.e. at y ₂ Left of axis and at a distance y ₂ The nearest sampling point of the shaft; as shown in FIG. 6 (b), if there is no position y ₂ Sampling points on the axis or left, i.e. without x _1Ri Sampling point less than or equal to 0, A _n Is determined as the leftmost sampling point, at which point A _n+1 Defined as the origin O of the coordinate system of the current following vehicle ₂ 。

Within each sampling period delta T, completing one-time following vehicle reference path planning, and guiding vehicle relative path { A } _i Sample point a of i=0, 1,2,..n+1 } _i Only keep A ₀ 、A ₁ 、……A _n 、A _n+1 Discard i>n+1 historical samples.

As shown in fig. 7, in this embodiment, the movement and rotation amount in the current sampling period of the following vehicle coordinate system is determined by the following vehicle speed and the following vehicle yaw rate of the following vehicle in the current sampling period, and the calculation formula of the movement and rotation amount in the current sampling period of the following vehicle coordinate system is as follows:

wherein DeltaO ₂ To follow the movement and rotation of the vehicle coordinate system in the current sampling period, deltax ₂ 、Δy ₂ 、θ ₂ For following the car coordinate system x ₂ O ₂ y ₂ Edge x within the current sampling period ₂ Axis, y ₂ The amount of movement of the shaft and the amount of rotation as a whole, ₁ In order to guide the yaw rate of the vehicle,ω ₂ for following the yaw rate of the vehicle, ΔT is the sampling period, F in FIG. 7 ₂ To follow the midpoint O of the front axle of the locomotive ₂ F ₂ The length is the wheelbase l of the head part between the following car and the guiding car ₂ 。

The embodiment shows a method for calculating a following vehicle reference path of a following vehicle, which specifically includes:

according to the movement and rotation quantity of the following vehicle coordinate system, the relative path { A 'of the guiding vehicle in the previous sampling period' _i I=0, 1,2,..n+1 } performs coordinate updating and advancing, and the specific formula is:

A _i+1 ＝A′ _i -ΔO ₂

equivalent to shifting all sampling points of the previous period by-delta O ₂ And adds 1 to the sample number.

Taking the relative pose q of the guided vehicle in the current sampling period as the initial point of the relative path of the guided vehicle in the current sampling period, namely the latest sampling point A of the relative path of the guided vehicle ₀ ，A ₀ =q; so far, the relative path { A { of the guided vehicle in the current sampling period is obtained _i |i＝0,1,2,...,n+1}。

For guided vehicle relative path { A }, using a fitting algorithm, e.g. Bezier fitting _i I=0, 1,2, n+1} performs an n+111 order fit to obtain a continuous follower reference path P (a) = [ x " _1RP (a),y _1RP (a),θ _1RP (a)] ^T Wherein a is [0,1 ]]The specific calculation formula is as follows:

A _i+1 ＝A′ _i -ΔO ₂ ,A ₀ ＝q

i＝0,1,...,n,n+1

in the formula, { A _i I=0, 1,2,..n+1 } is currentRelative path of guided vehicle in sampling period, { A' _i I=0, 1,2,..n+1 } is the lead vehicle relative path, Δo, for the last sample period ₂ For the movement and rotation of the following car coordinate system, q is the relative pose of the guiding car, a is the path parameter, x _1RP (a)、y _1RP (a)、θ _1RP (a) In a following vehicle coordinate system x for a following vehicle reference path P (a) ₂ O ₂ y ₂ And the abscissa and the angle of orientation, which are all functions of the path parameter a.

Thus, P (0) =a ₀ For the current sampling point, P (n+1) =a _n+1 The earliest sampling point is reserved; when a increases from 0 to 1, P (a) increases from A ₀ To A _n+1 And (5) smooth transition.

The above process is executed once every sampling period to obtain the real-time dynamically updated following vehicle reference path P (a) = [ x ] _1RP (a),y _1RP (a),θ _1RP (a)] ^T The automatic following control system is used for longitudinal and transverse control of the following vehicles so as to realize automatic following driving of the following vehicles in the logistics vehicle queue.

Further, the following vehicle planning and control system 5 further includes: a following vehicle longitudinal control module 53, a following vehicle lateral instability determination module 54, a following vehicle lateral control module 55;

the following vehicle longitudinal control module 53 is configured to construct a target equation x according to a following vehicle reference path _1RP (a) =0, and calculates a numerical solution a of the objective equation _y2 According to the numerical solution a _y2 The following vehicle reference path calculates the opening degree of an expected pedal of the following vehicle;

The following driving parameters in the automatic following driving process of the following vehicle in the embodiment at least comprise the expected pedal opening degree of the following vehicle and the expected steering column angle of the following vehicle, namely the automatic following driving process of the following vehicle is divided into transverse control and longitudinal control.

In the longitudinal control process of the following vehicle, the corresponding input quantity at least comprises: opening p of accelerator pedal of guide vehicle _acc1 Brake pedal opening p of guided vehicle _brk1 Longitudinal acceleration a of guided vehicle _{x1_IMU} Longitudinal speed v of the guided vehicle _x1 Longitudinal acceleration a of following vehicle _{x2_IMU} Longitudinal speed v of following vehicle _x2 And a guide car to follower car longitudinal track distance d _rel The corresponding output is the expected pedal opening of the following vehicle

Wherein (1)>

Indicating control of the brake pedal->

Indicating control of accelerator pedal->

The corresponding pedal desired opening is indicated and executed by an accelerator and brake pedal actuator of the following vehicle.

Specifically, in this embodiment, the longitudinal track distance d between the guided vehicle and the following vehicle is defined _rel In a following vehicle coordinate system x for a following vehicle reference path P (a) ₂ O ₂ y ₂ X in the middle _1RP (a) The length of the part is equal to or more than 0.

The method for determining the expected pedal opening of the following vehicle specifically comprises the following steps:

constructing a target equation x according to the following vehicle reference path _1RP (a) =0, and calculates a numerical solution a of the objective equation _y2 I.e. to make P (a _y2 ) The point falling at y ₂ On the shaft.

Then solve a according to the numerical value _y2 And a following vehicle reference path, calculating the longitudinal track distance d between the guiding vehicle and the following vehicle _rel The corresponding calculation formula is:

/>

as shown in fig. 8, the desired pedal opening of the following vehicle includes two parts, namely a pedal feedforward part and a kinematic state quantity feedback part, which are combined into a final desired pedal opening of the following vehicle according to a certain proportional relationship.

According to the longitudinal track distance d between the guiding vehicle and the following vehicle _rel The expected pedal opening of the following vehicle is calculated, and the corresponding calculation formula is as follows:

wherein K is _ff As a feed-forward scaling factor,

for a pedal feed-forward follower a pedal opening is desired, < >>

For kinematic feedback of the desired pedal opening of the following vehicle, K _a Longitudinal acceleration feedback coefficient, K _v Feedback coefficient, K of vehicle speed _d Longitudinal trajectory distance feedback coefficient, a _{x1_IMU} To guide the longitudinal acceleration of the vehicle, a _{x2_IMU} To follow the longitudinal acceleration of the vehicle v _x1 To guide the longitudinal speed of the vehicle v _x2 To follow the longitudinal speed of the vehicle, d _ref For guiding the longitudinal track distance reference value of the car and the following car.

Wherein the longitudinal acceleration feedback coefficient K _a Feedback coefficient K of vehicle speed _v Longitudinal trajectory distance feedback coefficient K _d The calculation formula of (2) is as follows:

wherein Δd _rel D is the longitudinal track distance following error _ref For guiding the longitudinal track distance reference value of the vehicle and the following vehicle, D _Δd For the longitudinal track distance to follow the deviation coefficient r _d For the longitudinal track distance to follow the deviation increase coefficient, v _{x2_low} 、v _{x2_high} The lower speed limit of the feedback coefficient regulation and the upper speed limit of the feedback coefficient regulation are respectively carried out; k (K) _{a_vl} 、K _{v_vl} 、K _{d_vl} Respectively a low-speed longitudinal acceleration feedback coefficient, a low-speed vehicle speed feedback coefficient and a low-speed longitudinal track distance feedback coefficient, K _{a_vh} 、K _{v_vh} 、K _{d_vh} The feedback coefficient is a high-speed longitudinal acceleration feedback coefficient, a high-speed vehicle speed feedback coefficient and a high-speed longitudinal track distance feedback coefficient respectively.

Longitudinal acceleration feedback coefficient K _a Feedback coefficient K of vehicle speed _v Longitudinal trajectory distance feedback coefficient K _d Is taken by the speed v of the following vehicle _x2 Following error Δd from longitudinal track distance _rel On the basis of which the longitudinal track distance following deviation coefficient D is continuously introduced _Δd The value range is D _Δd 1 or more for describing the longitudinal track distance d between the guided vehicle and the following vehicle _rel Deviation from the reference value d _ref Degree of Deltad _{rel_eff} A trigger threshold is set for the longitudinal track distance following error; when |d _rel I exceeds Δd _{rel_eff} At time D _Δd Starting to increase; r is (r) _d For the longitudinal track distance to follow the deviation increase coefficient, r _d >0 and r _d The larger D _Δd With |d _rel The faster the rate of increase.

Through the arrangement, K is formed _a 、K _v 、K _d Along withSpeed v of following vehicle _x2 The change is carried out so as to adapt to the low-speed working condition and the high-speed working condition respectively and smoothly transition; when the absolute value of the longitudinal track distance following error is |d _rel When I increases, the longitudinal track distance follows the deviation coefficient D _Δd Increase to K _a 、K _v Reduction, K _d The equal proportion increases, thereby strengthening the distance feedback, weakening the speed and acceleration feedback, so as to rapidly reduce the longitudinal track distance following error.

Similarly, the pedal feedforward follower expects a pedal opening

The calculation formula of (2) is as follows:

wherein p is _acc1 To guide the opening degree of the accelerator pedal of the vehicle, p _brk1 To guide the opening degree K of the brake pedal of the vehicle _pa 、K _pb The feedforward coefficient of the accelerator pedal and the feedforward coefficient of the brake pedal, p _{acc_eff} 、p _{brk_eff} The effective opening degree of the feedforward of the accelerator pedal and the effective opening degree of the feedforward of the brake pedal are respectively p _{brk_eff} 、p _{brk_sat} The opening degree of the feedforward saturation of the accelerator pedal and the opening degree of the feedforward saturation of the brake pedal are respectively.

Accelerator pedal feed-forward coefficient K _pa And brake pedal feedforward coefficient K _pb The pedal opening is a function of the corresponding pedal opening, and when the pedal opening is increased within a certain range, the corresponding pedal feedforward coefficient is also increased so as to respond quickly to the sudden acceleration and deceleration conditions of the guided vehicle.

In the following vehicle transverseIn the control process, the following vehicle follows the track of the following vehicle reference path, and whether the transverse control is unstable or not needs to be judged at any time so as to adjust the transverse control in time. Thus, to quantify the lateral instability condition, a track following lateral instability degree D is defined _us To ensure the accuracy and reliability of the following vehicle lateral control, i.e. the calculation of the desired steering column angle of the following vehicle.

The following vehicle transverse instability determination module 54 is configured to solve a according to the numerical value _y2 And a following vehicle reference path, calculating a following vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 And according to the following vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 Calculating the track following transverse instability degree D _us ；

Specifically, according to the numerical solution a _y2 And a following vehicle reference path, calculating a following vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 The corresponding calculation formula is:

/>

in the formula, the numerical solution a _y2 Is equation x _1RP (a) A numerical solution of =0, i.e. current time following the car reference path P (a) = [ x ] _1RP (a),y _1RP (a),θ _1RP (a)] ^T And y is ₂ Axis intersection point P (a) _y2 ) Is the independent variable of (a). Solving for a according to the value _y2 And a following vehicle reference path, the following vehicle transverse deviation e can be calculated _y2 Deviation e of following vehicle orientation _θ2 Following the vehicle lateral deviation e _y2 Refers to the transverse distance between the current following vehicle and the history path of the guiding vehicle, namely the following vehicle coordinate system x ₂ O ₂ y ₂ Reference paths P (a) and y of the middle follower vehicle ₂ An ordinate of the axis intersection; following vehicle orientation deviation e _θ2 Refers to an included angle of the head orientation of the current following parking pose and the historical pose of the guiding vehicle at the same transverse position.

According to the following vehicle transverse deviation e _y2 And following the car towardsDeviation e of direction _θ2 Calculating the track following transverse instability degree D _us The corresponding calculation formula is:

D _us ＝D _{us_yz} (e _yz )+D _{us_θ2} (e _θz )

when the following vehicle is deviated from the lateral direction e _y2 Deviation e of following vehicle orientation _θ2 At the corresponding effective deviation e _{y2_eff} 、e _{θ2_eff} Degree of track following lateral instability D _us 0, the following vehicle is considered to be not unstable in the transverse direction; when following the transverse deviation e of the vehicle _y2 Or following the deviation e of the vehicle orientation _θ2 Exceeding the corresponding effective deviation e _{y2_eff} 、e _{θ2_} Degree of lateral instability D of the track following _us With the increase, the transverse instability degree of the following vehicle tends to be serious; degree of lateral instability D when the track follows _us When the upper limit value 1 is reached, the following-vehicle lateral control is considered to have completely been destabilized.

The following vehicle transverse control module 55 is used for controlling the following vehicle transverse deviation e _y2 And degree of track following lateral instability D _us Determining a desired steering column angle for a follower

Specifically, as shown in fig. 9, in the following vehicle transverse direction control process, the input amounts include at least: lateral deviation e of following vehicle _y2 Longitudinal speed v of following vehicle _x2 Following vehicle reference path P (a) and guiding vehicle current position A ₀ (x _1R ,y _1R ) The method comprises the steps of carrying out a first treatment on the surface of the Wherein A is ₀ (x _1R ,y _1R ) Relative pose q= (x) by current lead vehicle _1R ,y _1R ,θ _1R ) ^T The corresponding output is the desired steering column angle of the following vehicle

Is executed by an electric control steering mechanism of the following vehicle.

The following vehicle transverse control is divided into two parts: the stable control part and the unstable control part synthesize the expected front wheel steering angle of the following vehicle according to a certain proportion relation

Wherein the degree of lateral instability D follows the track _us Is increased by the desired front wheel angle of the following vehicle +.>

Expected front wheel corner of following vehicle in middle instability state>

The proportion of the materials increases.

In particular, a steering column angle is desired for a steady state follower

It is fed back by the lateral deviation feedback part +.>

And relative path pre-aiming part corner +.>

The two parts are overlapped.

Expected rotation of following vehicleSteering column angle

The front wheel corner is expected by the following vehicle>

The proportional amplification is carried out, and the corresponding calculation formula is as follows:

/>

wherein K is _s As a coefficient of the steering proportion,

for following the desired front wheel angle of the vehicle +.>

For the stable state following the expected front wheel turning angle of the vehicle, the unstable state following the expected front wheel turning angle +.>

For the lateral deviation feedback portion,

In the unstable state, the following vehicle expects a front wheel steering angle

For direct pre-aiming of the current position A of the guided vehicle ₀ (x _1R ,y _1R ) The specific formula is:

as shown in fig. 10, the relative path pre-aiming portion turns

Is defined as selecting a point T on the current follower reference path P (a) ₂ Pre-aiming is carried out to ensure that the following vehicle just passes through T ₂ Front wheel corner at point, relative path pre-aiming part corner

The calculation method of (1) specifically comprises:

according to the longitudinal speed v of the following vehicle _x2 Calculating the pre-aiming distance d of the following vehicle _PV (v _x2 ) The corresponding calculation formula is:

wherein d _{PV_low} For low speed pretarge distance v _{x2_pV} Increasing the initial vehicle speed for the pre-aiming distance, K _PV Is a pretightening coefficient;

according to the pre-aiming distance d of the following vehicle _PV (v _x2 ) The track following vehicle reference path is used for constructing a pre-aiming distance equation, and a pre-aiming point T of the track following vehicle on the track following vehicle reference path is calculated ₂ Coordinates (x) _T2 ,y _T2 ) The corresponding calculation formula is:

wherein a is _T2 A numerical solution of a pretightening distance equation;

according to the pre-aiming point T ₂ Is the longitudinal coordinate y of (2) _T2 And a pre-aiming distance d of the following vehicle _PV (v _x2 ) Calculating the rotation angle of the pre-aiming part of the relative path

The corresponding calculation formula is:

wherein, I ₂ Is the wheelbase of the head part between the following car and the guiding car.

In this embodiment, when the lateral control of the following vehicle is in a stable state, the pre-aiming point tends to be determined at a relatively close position on the reference path P (a) of the following vehicle and the lateral error of track following is corrected in real time, so as to drive the following vehicle to travel along the history path of the guiding vehicle; when the transverse control of the following vehicle is in an unstable state, the fact that the reconstruction of the reference path of the following vehicle is problematic or the path following is difficult is meant, the current position of the guiding vehicle tends to be directly pre-aimed, the following vehicle directly follows the history path of the guiding vehicle instead of the guiding vehicle, the stability of the transverse control is enhanced, and the unstable state of the transverse control is gradually changed.

The system 100 further includes: the following vehicle control execution system 6 is used for controlling the following vehicle to automatically follow driving according to following driving parameters, wherein the following driving parameters at least comprise a following vehicle expected pedal opening degree and a following vehicle expected steering column angle.

The implementation of the following vehicle control execution system 6 is not limited in this embodiment.

Embodiment two:

on the basis of the above embodiment, the present embodiment provides a following type automatic driving logistics vehicle control method, which is suitable for automatic driving control of following vehicles in a logistics vehicle queue, wherein the logistics vehicle queue includes a guiding vehicle and at least one following vehicle, the guiding vehicle is connected with the following vehicle and the following vehicle is connected with the preceding following vehicle through a vehicle-vehicle connection mechanism, UWB positioning systems are arranged on the guiding vehicle and the following vehicle, and the method includes:

Step 1, calculating the relative pose of a guided vehicle in a following vehicle coordinate system according to acquired vehicle-vehicle relative positioning information and vehicle state information, wherein the vehicle-vehicle relative positioning information is determined by a vehicle-vehicle relative positioning system, and the vehicle-vehicle relative positioning system comprises a UWB positioning system, a guided vehicle head and cargo compartment included angle measuring device, a vehicle-vehicle connecting mechanism and the like;

the implementation of the vehicle-to-vehicle positioning system according to the embodiment is not limited.

The vehicle-to-vehicle relative positioning information includes: relative distance d of UWB positioning system _U Azimuth angle theta of UWB positioning system _UO And UWB positioning system attitude angle theta _UA Horizontal included angle theta between guide vehicle and longitudinal axis of trailer _1P Length d of vehicle-to-vehicle communication connection line _G Horizontal angle theta of vehicle-to-vehicle communication connecting line and central axis of guide vehicle container _G1 And the horizontal angle theta between the vehicle head central axis and the following vehicle _G2 。

Specifically, as shown in fig. 3, UWB positioning units on the left side and the right side of the tail of the container of the guided vehicle are respectively (1) and (2), and UWB positioning units on the head of the following vehicle are respectively (3) and (3)(4) A number; UWB positioning system relative distance d _U The length of the connecting line between the midpoint of (1) and (2) and the midpoint of (3) and (4) is the azimuth angle theta of the UWB positioning system _UO The attitude angle theta of the UWB positioning system is the included angle between the midpoint of the (1), the midpoint of the (2), the midpoint of the (3), the midpoint of the (4), the midpoint of the (3) and the perpendicular bisector of the (4) _UA Is the included angle between the perpendicular bisectors (1), (2) and (3) and (4).

The vehicle state information includes: opening p of accelerator pedal of guide vehicle _acc1 Brake pedal opening p of guided vehicle _brk1 Accelerator pedal opening p of following vehicle _acc2 Brake pedal opening p of following vehicle _brk2 Steering column angle delta of guided vehicle _s1 Steering column angle delta of follower _s2 Longitudinal acceleration a of guided vehicle _{x1_IMU} Yaw rate acquisition value omega of guide vehicle _{1_IMU} Longitudinal acceleration a of following vehicle _{x2_IMU} Yaw rate acquisition value omega of following vehicle _{2_IMU} Longitudinal speed v of the guided vehicle _x1 Longitudinal speed v of following vehicle _x2 。

The embodiment shows a method for calculating the relative pose q of a guided vehicle, which specifically comprises the following steps:

first, according to the steering column angle delta of the following vehicle _s2 The rotation angle delta of the front wheel of the following car is obtained through conversion _w2 And is combined withLongitudinal speed v of the hybrid following vehicle _x2 Calculating a yaw rate calculation value omega of the following vehicle _{2_KIN} 。

ω _{2_KIN} ＝v _x2 tanδ _w2 /l ₂

wherein, I ₂ Delta for the wheelbase of the head part between the follower and the lead vehicle _w2 To follow the front wheel angle, v _x2 Is the longitudinal speed of the following vehicle, wherein the rotation angle delta of the front wheel of the following vehicle _w2 Steering column angle delta of follower _s2 The conversion relation is as follows:

δ _w2 ＝K _s δ _s2

wherein K is _s Is the steering scaling factor.

ω ₂ ＝(1-K _yaw )ω _{2_KIN} +K _yaw ω _{2_IMU}

In this embodiment, the following vehicle yaw rate acquisition value ω _{2_IMU} The sensor (such as a motion combination sensor) can directly measure the data, and the data is accurate but contains larger noise; following vehicle yaw rate calculation value omega calculated by vehicle kinematics model _{2_KIN} The noise is small, but a certain error exists under the influence of the clearance of the steering transmission mechanism, the tire slip angle and the like. Therefore, in order to improve the accuracy of the calculation of the yaw rate of the following vehicle, a weighting algorithm is adopted to acquire a value omega of the yaw rate of the following vehicle _{2_IMU} Calculated value ω of yaw rate of following vehicle _{2_KIN} And performing weighting operation.

Thus, according to the following vehicle longitudinal speed v _x2 Determining the weight values of the two, and enlarging omega in the low-speed stage of the vehicle with more accurate kinematic model _{2_KIN} And omega is increased in a high-speed stage of the vehicle in which sensor noise is less affected _{2_IMU} Is a weight of (a). When v _x2 <v _low Or v _x2 ≥v _high When the data are not fused, omega is directly adopted _{2_KIN} Or omega _{2_IMU} As the final yaw rate ω ₂ The method ensures the authenticity of the yaw rate of the following vehicle, and further improves the accuracy of following control of the following vehicle.

And (5) performing fusion calculation.

the specific formula is as follows:

The specific formula is as follows:

in the method, in the process of the invention,

The specific formula is as follows:

wherein q '= (x' _1R ,y′ _1R ,θ′1 _R ) ^T The relative pose of the guide vehicle is the previous sampling period.

UWB-based determination according to FIG. 5 (b)Relative pose measurement q of guide vehicle of position system _U ＝(x _U1R ,y _U1R ,θ _U1R ) ^T The calculation formula of (2) is as follows:

wherein, I _2UO Is from the midpoint of the UWB positioning system (3) and (4) to the origin O of the following vehicle coordinate system ₂ Is a horizontal distance of (2); l (L) _1PU The horizontal distance from the rotating shaft of the traction seat of the guide vehicle to the midpoint of the No. 1 and the No. 2 UWB positioning systems; l (L) _1PO For the rotation shaft of the traction seat of the guide vehicle to the origin O of the coordinate system of the guide vehicle ₁ Is a horizontal distance of (c).

q＝K _q q _u +(1-K _q )q _G

k _g ＝(K _qx +K _qy +K _qθ )/3

that is, the present embodiment is in progressWhen the relative pose of the guided vehicle is fused and calculated, the relative pose measured value q of the guided vehicle is measured according to the UWB positioning system _U Relative guide vehicle pose q of vehicle-vehicle connecting mechanism _G Pose estimation value relative to guide vehicle

Step 2, taking the relative pose of the guided vehicle as the initial point of the guided vehicle relative path of the current sampling period, updating the guided vehicle relative path of the current sampling period according to the guided vehicle relative path of the previous sampling period and the movement and rotation quantity of the current sampling period following vehicle coordinate system, determining the following vehicle reference path of the following vehicle through a fitting algorithm,

In this embodiment, the following vehicle reference path planning is based on the relative path reconstruction of the lead vehicle. As shown in fig. 6, the guided vehicle relative path { a } _i I=0, 1,2,..n+1 } is referred to in the current following car coordinate system x ₂ O ₂ y ₂ Guiding a set of historical relative poses of the vehicle; because the sampling exists at time intervals, the actually collected relative path of the guide vehicle consists of discrete sampling points, A is used for _i (i=0, 1, 2.) represents, a _i ＝(x _1Ri ,y _1Ri ,θ _1Ri ) ^T Contains three elements, namely, the origin O of the coordinate system of the guided vehicle at the sampling time ₁ In the current following car coordinate system x ₂ O ₂ y ₂ Middle abscissa x _1ri 、y _1Ri And sampling time x ₁ Axis and current time x ₂ Included angle theta of axes _1Ri 。

Let the sampling period be DeltaT, A ₀ ＝(x _1R ,y _1R ,θ _1R ) ^T The sampling point at the current moment is equivalent to the relative pose q of the guiding vehicle at the current moment; a is that _i ＝(x _1Ri ,y _1Ri ,θ _1Ri ) ^T A sampling point that is i deltat, i.e. that precedes i samples, is currently followed byVehicle coordinate system x ₂ O ₂ y ₂ Is a pose of the model (a).

wherein DeltaO ₂ To follow the movement and rotation of the vehicle coordinate system in the current sampling period, deltax ₂ 、Δy ₂ 、Δθ ₂ For following the car coordinate system x ₂ O ₂ y ₂ Edge x within the current sampling period ₂ Axis, y ₂ The amount of movement of the shaft and the amount of rotation as a whole, ₁ To guide the yaw rate of the vehicle omega ₂ For following the yaw rate of the vehicle, ΔT is the sampling period, F in FIG. 7 ₂ To follow the midpoint O of the front axle of the locomotive ₂ F ₂ The length is the wheelbase l of the head part between the following car and the guiding car ₂ 。

A _i+1 ＝A′ _i -ΔO ₂

For guided vehicle relative path { A }, using a fitting algorithm, e.g. Bezier fitting _i I=0, 1,2, n+1} performs an n+1 order fit to obtain a continuous follower reference path P (a) = [ x _1RP (a),y _1RP (a),θ _1RP (a)] ^T Wherein a is [0,1 ]]The specific calculation formula is as follows:

A _i+1 ＝A′ _i -ΔO ₂ ,A ₀ ＝

i＝0,1,...,n,n+1

in the formula, { A _i I=0, 1,2,..n+1 } is the lead car relative path for the current sampling period, { a }. _i I=0, 1,2,..n+1 } is the lead vehicle relative path, Δo, for the last sample period ₂ For the movement and rotation of the following car coordinate system, q is the relative pose of the guiding car, a is the path parameter, x _1RP (a)、y _1RP (a)、θ _1RP (a) In a following vehicle coordinate system x for a following vehicle reference path P (a) ₂ O ₂ y ₂ And the abscissa and the angle of orientation, which are all functions of the path parameter a.

And 3, determining following running parameters of the following vehicle according to the following vehicle reference path and the vehicle state information, wherein the following running parameters at least comprise the expected pedal opening degree of the following vehicle and the expected steering column angle of the following vehicle.

Wherein (1)>

Indicating control of the brake pedal->

Indicating control of accelerator pedal->

step 31, constructing a target equation x according to the following vehicle reference path _1RP (a) =0, and calculates a numerical solution a of the objective equation _y2 I.e. to make P (a _y2 ) The point falling at y ₂ On the shaft.

wherein K is _ff As a feed-forward scaling factor,

for a pedal feed-forward follower a pedal opening is desired, < >>

For kinematic feedback of the desired pedal opening of the following vehicle, K _a Longitudinal acceleration feedback coefficient, K _v Feedback coefficient, K of vehicle speed _d Longitudinal trajectory distance feedback coefficient, a _{x1_} To guide the longitudinal acceleration of the vehicle, a _{x2_IMU} To follow the longitudinal acceleration of the vehicle v _x1 To guide the longitudinal speed of the vehicle v _x2 To follow the longitudinal speed of the vehicle, d _ref For guiding the longitudinal track distance reference value of the car and the following car.

Δd _rel ＝d _ref -d _rel

Longitudinal acceleration feedback coefficient K _a Feedback coefficient K of vehicle speed _v Longitudinal trajectory distance feedback coefficient K _d Is taken by the speed v of the following vehicle _x2 Following error Δd from longitudinal track distance _rel On the basis of which the longitudinal track distance following deviation coefficient D is continuously introduced _Δd The value range is D _Δd 1 or more for describing the longitudinal track distance d between the guided vehicle and the following vehicle _rel Deviation from the reference value d _ref Degree of Deltad _{rel_eff} A trigger threshold is set for the longitudinal track distance following error; when |Δd _rel I exceeds Δd _{rel_eff} At time D _Δd Starting to increase; r is (r) _d For the longitudinal track distance to follow the deviation increase coefficient, r _d >0 and r _d The larger D _Δd With |Δd _rel The faster the rate of increase.

Through the arrangement, K is formed _a 、K _v 、K _d With the speed v of the following vehicle _x2 The change is carried out so as to adapt to the low-speed working condition and the high-speed working condition respectively and smoothly transition; when the absolute value of the longitudinal track distance following error is |Deltad _rel When I increases, the longitudinal track distance follows the deviation coefficient D _Δd Increase to K _a 、K _v Reduction, K _d The equal proportion increases, thereby strengthening the distance feedback, weakening the speed and acceleration feedback, so as to rapidly reduce the longitudinal track distance following error.

Similarly, the pedal feedforward follower expects a pedal opening

The calculation formula of (2) is as follows:

Accelerator pedal feed-forward coefficient K _pa And brake pedal feedforward coefficient K _pb Is a function of the corresponding pedal opening, and when the pedal opening is increased within a certain range, the corresponding pedal feedforward coefficient is also increased so as toAnd the rapid response is made to the rapid acceleration and rapid deceleration conditions of the guided vehicle.

In the transverse control process of the following vehicle, the following vehicle follows the track of the reference path of the following vehicle, and whether the transverse control is unstable or not needs to be judged at any time so as to adjust the transverse control in time. Thus, to quantify the lateral instability condition, a track following lateral instability degree D is defined _us To ensure the accuracy and reliability of the following vehicle lateral control, i.e. the calculation of the desired steering column angle of the following vehicle.

The embodiment shows a method for determining a desired steering column angle of a follower, which specifically includes:

Step 33, solving a according to the numerical value _y2 And a following vehicle reference path, calculating a following vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 The corresponding calculation formula is:

D _us ＝D _{us_y2} (e _y2 )+D _{us_θ2} (e _θ2 )

In particular, when following the vehicle lateral deviation e _y2 Deviation e of following vehicle orientation _θ2 At the corresponding effective deviation e _{y2_eff} 、e _{θ2_eff} Degree of track following lateral instability D _us 0, the following vehicle is considered to be not unstable in the transverse direction; when following the transverse deviation e of the vehicle _y2 Or following the deviation e of the vehicle orientation _θ2 Exceeding the corresponding effective deviation e _{y2_eff} 、e _{θ2_eff} Degree of lateral instability D of the track following _us With the increase, the transverse instability degree of the following vehicle tends to be serious; degree of lateral instability D when the track follows _us When the upper limit value 1 is reached, the following-vehicle lateral control is considered to have completely been destabilized.

Is executed by an electric control steering mechanism of the following vehicle.

Expected front wheel corner of following vehicle in middle instability state>

The proportion of the materials increases.

In particular, a steering column angle is desired for a steady state follower

It is fed back by the lateral deviation feedback part +.>

And relative path pre-aiming part corner +.>

The two parts are overlapped.

Desired steering column angle for follower

By followingVehicle expects front wheel corner +.>

/>

wherein K is _s As a coefficient of the steering proportion,

for following the desired front wheel angle of the vehicle +.>

For the lateral deviation feedback portion,

as shown in fig. 10, the relative path pre-aiming portion turns

The calculation method of (1) specifically comprises:

wherein a is _T2 A numerical solution of a pretightening distance equation;

The corresponding calculation formula is:

The technical scheme of the present application is described in detail above with reference to the accompanying drawings, and the present application provides a following type automatic driving logistics vehicle control system and method, the system is applicable to the automatic driving control of following vehicles in a logistics vehicle queue, the logistics vehicle queue comprises a guiding vehicle and at least one following vehicle, and the system comprises: the system comprises a vehicle-vehicle relative positioning system, a vehicle state information acquisition system, a following vehicle planning and control system and a following vehicle control execution system; the vehicle-vehicle relative positioning system is used for collecting vehicle-vehicle relative positioning information; the vehicle state information acquisition system is used for acquiring vehicle state information of the guide vehicle and the following vehicle; the following vehicle planning and control system is used for calculating the relative pose of the guided vehicle in a following vehicle coordinate system according to the vehicle relative positioning information and the vehicle state information, determining a following vehicle reference path of the following vehicle through a fitting algorithm, and determining following running parameters of the following vehicle according to the following vehicle reference path and the vehicle state information; the following vehicle control execution system is used for controlling the following vehicle to automatically follow driving according to following driving parameters, wherein the following driving parameters at least comprise the expected pedal opening degree of the following vehicle and the expected steering column angle of the following vehicle. Through the technical scheme in this application, with the operation scene dimension reduction of high autopilot truck for having the supervision formula to follow autopilot scene, realize a novel guide-autopilot train system of low cost, satisfy logistics industry wisdom transformation demand more.

The steps in the present application may be sequentially adjusted, combined, and pruned according to actual requirements.

The units in the device can be combined, divided and pruned according to actual requirements.

Although the present application is disclosed in detail with reference to the accompanying drawings, it is to be understood that such descriptions are merely illustrative and are not intended to limit the application of the present application. The scope of the present application is defined by the appended claims and may include various modifications, alterations, and equivalents to the invention without departing from the scope and spirit of the application.

Claims

1. A following autopilot logistics vehicle control system, wherein said system is adapted for autopilot control of following vehicles in a logistics vehicle consist comprising a lead vehicle and at least one of said following vehicles, said system comprising: the system comprises a vehicle-vehicle relative positioning system (3), a vehicle state information acquisition system (4), a following vehicle planning and control system (5) and a following vehicle control execution system (6);

the vehicle-vehicle relative positioning system (3) is used for collecting vehicle-vehicle relative positioning information;

the vehicle state information acquisition system (4) is used for acquiring vehicle state information of the guide vehicle and the following vehicle;

The following vehicle planning and control system (5) is used for calculating the relative pose of the guiding vehicle in a following vehicle coordinate system according to the vehicle relative positioning information and the vehicle state information, determining a following vehicle reference path of the following vehicle through a fitting algorithm, and determining following running parameters of the following vehicle according to the following vehicle reference path and the vehicle state information;

the following vehicle control execution system (6) is used for controlling the following vehicle to automatically follow driving according to the following driving parameters, wherein the following driving parameters at least comprise an expected pedal opening degree of the following vehicle and an expected steering column angle of the following vehicle;

the following vehicle planning and control system (5) comprises a guide vehicle relative pose calculating module (51), wherein the guide vehicle relative pose calculating module (51) is used for calculating the relative pose of the guide vehicle, and the calculating process of the relative pose of the guide vehicle specifically comprises the following steps:

steering column angle delta of guide vehicle respectively according to vehicle state information _s1 Steering column angle delta of follower _s2 Longitudinal speed v of the guided vehicle _x1 Longitudinal speed v of following vehicle _x2 Calculating yaw rate omega of guided vehicle ₁ Yaw rate omega of following vehicle ₂ ；

According to the yaw rate omega of the guided vehicle ₁ Yaw rate ω with the following vehicle ₂ Calculating the relative pose estimation value of the guided vehicle, and according to the relative pose estimation value of the guided vehicle

according to the steering column angle delta of the following vehicle _s2 The rotation angle delta of the front wheel of the following car is obtained through conversion _w2 In combination with the longitudinal speed v of the following vehicle _z2 Calculating the yaw rate of the following vehicleDegree calculation value omega _{2_KIN} ；

According to the steering angle delta of the front wheel of the following car _w2 Longitudinal speed v of following vehicle _x2 Deriving a calculated yaw rate omega of the following vehicle _{2_KIN} The formula is as follows:

ω _{2_KIN} ＝v _x2 tanδ _w2 /l ₂

δ _w2 ＝K _s δ _s2

wherein K is _s Is a steering proportionality coefficient;

ω ₂ ＝(1-K _yaw )ω _{2_KIN} +K _yaw ω _{2_IMU}

2. The following autopilot logistics vehicle control system of claim 1, wherein the follower planning and control system (5) further comprises a follower reference path construction module (52), the follower reference path construction module (52) being for determining a follower reference path of the follower, in particular comprising:

according to the movement and rotation quantity of the following vehicle coordinate system, updating and advancing the coordinates of the relative path of the guiding vehicle in the previous sampling period;

taking the relative pose of the guide vehicle in the current sampling period as an initial point of the relative path of the guide vehicle in the current sampling period;

and fitting the relative paths of the guided vehicles by using a fitting algorithm, and determining the reference paths of the following vehicles.

3. The following autopilot logistics vehicle control system of claim 1, wherein the follower planning and control system (5) further comprises: a following vehicle longitudinal control module (53), a following vehicle transverse instability judging module (54), a following vehicle transverse control module (55);

The following vehicle longitudinal control module (53) is used for constructing a target equation x according to the following vehicle reference path _1RP (a) =0, and calculating a numerical solution a of the objective equation _y2 Solving for a according to the value _y2 The following vehicle reference path calculates the opening degree of an expected pedal of the following vehicle;

the following vehicle transverse instability judging module (54) is used for solving a according to the numerical value _y2 And the following vehicle reference path, calculating the following vehicle transverse deviation e _y2 Deviation e of following vehicle orientation _θ2 And according to the following vehicle transverse deviation e _y2 And the following vehicle orientation deviation e _θ2 Calculating the track following transverse instability degree D _us ；

The following transverse control module (55) is used for controlling the following transverse deviation e according to the following transverse deviation e _y2 And the degree of track following lateral instability D _us Determining the desired steering column angle of the follower

4. A following autopilot logistics vehicle control system in accordance with any one of claims 1 to 3, wherein the relative positioning system (3) further comprises: a vehicle-to-vehicle connection mechanism (33);

the guiding vehicle is connected with the following vehicle and two adjacent following vehicles through the vehicle-vehicle connecting mechanism (33), and the vehicle-vehicle connecting mechanism (33) is used for wired data transmission.

5. A method for controlling a following automated guided logistics vehicle, the method being adapted for automated driving control of a following vehicle in a logistics vehicle consist, the logistics vehicle consist comprising a lead vehicle and at least one of the following vehicles, the method comprising:

step 1, calculating the relative pose of the guided vehicle in a following vehicle coordinate system according to acquired vehicle-to-vehicle relative positioning information and vehicle state information;

the guiding vehicle is connected with the following vehicle and the following vehicle is connected with the previous following vehicle through a vehicle-vehicle connecting mechanism, a UWB positioning system is arranged on the guiding vehicle and the following vehicle, and the relative pose q of the guiding vehicle is calculated in the step 1, and the method specifically comprises the following steps:

step 11, respectively according to the steering column angle delta of the guiding vehicle in the vehicle state information _s1 Steering column angle delta of follower _s2 Longitudinal speed v of the guided vehicle _x1 Longitudinal speed v of following vehicle _x2 Calculating yaw rate omega of guided vehicle ₁ Yaw rate omega of following vehicle ₂ ；

Step 12, according to the yaw rate omega of the guided vehicle ₁ Yaw rate ω with the following vehicle ₂ Calculating the relative pose estimation value of the guided vehicle, and according to the relative pose estimation value of the guided vehicle

Relative pose measurement q of guide vehicle of UWB positioning system _U Relative guide vehicle pose q of vehicle-vehicle connecting mechanism _G Performing fusion calculation to generate a relative pose q of the guide vehicle; calculating the yaw rate omega of the following vehicle ₂ Specifically comprises the following steps: />

According to the following vehicle rotationSteering column angle delta _s2 The rotation angle delta of the front wheel of the following car is obtained through conversion _w2 And combine the longitudinal speed v of the following vehicle _x2 Calculating a yaw rate calculation value omega of the following vehicle _{2_KIN} ；

Acquiring a value omega according to the yaw rate of the following vehicle in the vehicle state information _{2_IMU} And the following vehicle yaw rate calculation value ω _{2_KIN} Calculating the yaw rate omega of the following vehicle ₂ Yaw rate ω of the following vehicle ₂ The calculation formula of (2) is as follows:

ω ₂ ＝(1-K _yaw )ω _{2_KIN} +K _yaw ω _{2_IMU}

wherein K is _yaw To blend the proportionality coefficients for yaw rate, v _low And v _high The lower limit vehicle speed and the upper limit vehicle speed are respectively the yaw rate fusion; step 2, taking the relative pose of the guided vehicle as the initial point of the guided vehicle relative path of the current sampling period, updating the guided vehicle relative path of the current sampling period according to the guided vehicle relative path of the previous sampling period and the movement and rotation quantity of the following vehicle coordinate system of the current sampling period, determining the following vehicle reference path of the following vehicle through a fitting algorithm,

The movement and rotation quantity of the following vehicle coordinate system is determined by the following vehicle speed and the following vehicle yaw rate of the following vehicle in the current sampling period;

and 3, determining following running parameters of the following vehicle according to the following vehicle reference path and the vehicle state information, wherein the following running parameters at least comprise an expected pedal opening degree of the following vehicle and an expected steering column angle of the following vehicle.

6. The following autopilot logistics vehicle control method of claim 5, wherein in step 3, the method of determining the desired pedal opening of the following vehicle comprises:

step 31, constructing a target equation x according to the following vehicle reference path _1RP (a) =0, and calculating a numerical solution a of the objective equation _y2 Solving for a according to the value _y2 The following vehicle reference path calculates the longitudinal track distance d between the guiding vehicle and the following vehicle _rel The corresponding calculation formula is:

step 32, according to the longitudinal track distance d between the guiding vehicle and the following vehicle _rel Calculating the opening degree of the expected pedal of the following vehicle, wherein the corresponding calculation formula is as follows:

wherein K is _ff As a feed-forward scaling factor,

for a pedal feed-forward follower a pedal opening is desired, < >>

For kinematic feedback of the desired pedal opening of the following vehicle, K _a Longitudinal acceleration feedback coefficient, K _v Feedback coefficient, K of vehicle speed _d Longitudinal trajectory distance feedback coefficient, a _x1 To guide the longitudinal acceleration of the vehicle, a _x2 To follow the longitudinal acceleration of the vehicle v _x1 To guide the longitudinal speed of the vehicle v _x2 To follow the longitudinal speed of the vehicle, d _ref For guiding the longitudinal track distance reference value of the car and the following car.

7. The method for controlling a following autopilot logistics vehicle as set forth in claim 6, wherein in step 3, the method for determining the desired steering column angle of the following vehicle comprises:

step 33, solving a according to the numerical value _y2 And the following vehicle reference path, calculating the following vehicle transverse deviation e _y2 Deviation e of following vehicle orientation _θ2 ；

Step 34, according to the following vehicle lateral deviation e _y2 And the following vehicle orientation deviation e _θ2 Calculating the track following transverse instability degree D _us The corresponding calculation formula is:

D _us ＝D _{us_y2} (e _y2 )+D _{us_θ2} (e _θ2 )

step 35, according to the following vehicle lateral deviation e _y2 And the degree of track following lateral instability D _us Determining the desired steering column angle of the follower

The corresponding calculation formula is:

wherein K is _s As a coefficient of the steering proportion,

for following the desired front wheel angle of the vehicle +.>

For the lateral deviation feedback part->

Pretarget part of the corner for the relative path, K _{P_ey} 、K _{I_ey} 、K _{D_ey} The proportional coefficient, the integral coefficient and the differential coefficient of the lateral deviation feedback are respectively. />