CN115717898A - Automatic driving planning trajectory perfect deduction method for commercial articulated vehicle - Google Patents
Automatic driving planning trajectory perfect deduction method for commercial articulated vehicle Download PDFInfo
- Publication number
- CN115717898A CN115717898A CN202211385064.9A CN202211385064A CN115717898A CN 115717898 A CN115717898 A CN 115717898A CN 202211385064 A CN202211385064 A CN 202211385064A CN 115717898 A CN115717898 A CN 115717898A
- Authority
- CN
- China
- Prior art keywords
- order
- deduction
- vehicle
- trailer
- follows
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a perfect deduction method of a commercial articulated vehicle automatic driving planning track, wherein a deduction device of the method receives the planning track issued by a planning module in real time, expresses the planning track through a space trajectory fitting coefficient and a real-time expected acceleration, perfectly deduces zero-order pose, first-order and second-order kinematic quantities related to the position of the mass center of a traction vehicle body, estimates an articulation angle and first-order and second-order change rates thereof, perfectly deduces the zero-order pose, first-order and second-order kinematic quantities related to the position of the mass center of a trailer body, inputs the kinematic quantities of each order of a tractor and the trailer into a traffic scene deduction program, and inputs data of the whole deduction process into a visualization and evaluation module for analysis and evaluation of an automatic driving algorithm and a tester. The invention adopts the mode of 'space trajectory + real-time expected acceleration' to perform perfect deduction, can simultaneously realize perfect deduction of zero-order pose and first-order and second-order motion information of the trailer body and the trailer body, and has strong practicability.
Description
Technical Field
The invention belongs to the technical field of automatic driving simulation of commercial vehicles, and particularly relates to a perfect deduction method for an automatic driving planning track of a commercial articulated vehicle.
Background
The automatic driving planning trajectory needs to meet the requirements of safety, comfort, competitive type, intelligence, physical feasibility and the like in various complex traffic scenes. According to the automatic and killing planning track, the situation that the perfect control is achieved can be determined, and the time and the posture of a reference track point fixedly connected with the vehicle body can be determined to be at the position and the position under the situation that the perfect control is achieved. Although there is a certain deviation between the actual control-realized trajectory and the planned trajectory, the quality of the planned trajectory itself determines the upper limit of the execution effect of the automatic driving control. In the algorithm development stage, various complex traffic scenes need to be combined, the automatic driving planning track is perfectly deduced through the simulation platform, and the deduction result is visualized and subjected to data post-processing analysis, so that various performances of the planning track are comprehensively evaluated, and the planning algorithm is guided to be updated in a generation-falling mode. Therefore, the perfect deduction method of the automatic driving planning track forms an important technical support for developing and testing the automatic driving algorithm. The existing perfect deduction steps of the autodrive planning trajectory are shown in fig. 1. In each deduction cycle, the specific steps are as follows:
and 4, respectively simulating and calculating zero-order pose information, first-order motion information and second-order motion information of the current vehicle according to current track point reference information obtained by interpolation of adjacent reference points, wherein the zero-order pose information comprises vehicle position coordinates x, y and orientation h, and the first-order motion information comprises the longitudinal speed v of the vehicle x Transverse velocity v y And yaw angular velocity ω, the second order motion information including longitudinal acceleration a x Lateral acceleration a y And yaw angular velocity α;
and 5, inputting the deduction result of each order of the kinematic quantity of the vehicle into a traffic scene deduction program, wherein the traffic scene deduction program is combined with the dynamic scene file and the static map information, each traffic participant generates an interactive behavior with the motion trail of the main vehicle, and the whole process data is finally input into a visualization and evaluation module for automatic driving algorithm development and analysis and evaluation of testers.
And finally, when the deduction termination time condition is established, circularly exiting, and finishing the automatic planning track perfect deduction process of the commercial articulated vehicle.
The existing automatic driving planning trajectory perfect deduction method has the following defects:
(1) In the existing perfect deduction method for the planned track of automatic driving, a main vehicle is suitable for a two-axle passenger vehicle, and for a commercial articulated vehicle, the existing method cannot perform differential deduction on the kinematic tracks of a tractor body and a trailer body because the articulated angle cannot be estimated. Under the steering driving condition, the trajectory deduction result of neglecting the hinge angle is practically impossible to realize, and the misjudgment of the planned trajectory effect is inevitably caused.
(2) The existing method has strict requirements on the expression mode of the planning track, must express a complete space-time planning track, and has great limitation. The conventional method cannot perfectly deduce a planning track expression mode of 'space trajectory + real-time expected acceleration' adopted in actual automatic driving of the commercial vehicle.
Disclosure of Invention
The invention aims to provide a perfect deduction method for an automatic driving planning track of a commercial articulated vehicle, aiming at the problems in the prior art.
In order to achieve the above object, the present invention provides a perfect deduction method for an automatic driving planning trajectory of a commercial articulated vehicle, comprising the steps of:
v p (k)＝v p (k-1)+T s a trg (k)
wherein, T s A time step for trajectory deduction;
step 11, perfectly deducing the zero-order pose, the first-order kinematic quantity and the second-order kinematic quantity related to the position of the mass center of the trailer body according to the deduced motion information of each order of the traction vehicle body in the step 9 and the deduced motion information of each order of the articulation angle in the step 10;
and 12, inputting the kinematic quantities of each step of the tractor and the trailer obtained by deduction in the steps 9 and 11 into a traffic scene deduction program, and inputting the data of the whole deduction process into a visualization and evaluation module for automatic driving algorithm development and analysis and evaluation of testers.
The invention further adopts the following technical scheme:
in step 1, the specific expression manner of the planned trajectory is as follows: in each control execution period, the planning module sends out expected space trajectories (x (p), y (p)) of the vehicle body planning reference point and expected acceleration information at the current moment; the vehicle body planning reference point is generally selected as a central point of a driving shaft of the tractor, the coordinates (x, y) of the expected space trajectory are described in the direction of a coordinate system of the vehicle, the parameter p is continuously taken between [0,1], and the mathematical equation of the expected space trajectory is as follows:
wherein, a i ,b i (i =0,1,2,3,4) is a fitting coefficient, and the planning module sends an expected acceleration command a to the control module in real time according to the current feedback vehicle speed and the planned driving target trg (k)。
In step 2, the conversion equation is as follows:
the space trajectory parameter equation under the global coordinate system obtained after conversion is as follows:
in the step 3, the distance discrete sequence s is obtained by sampling vec Is about a parameter discrete sequence p vec While the parameter is a discrete sequence p vec Also with respect to the distance discrete sequence s vec The two are in one-to-one correspondence.
In the step 4, firstly, the following mathematical optimization problem is solved to obtain the corresponding parameter p of the nearest point nearest :
Then the driving distance s constructed by the step 3 vec And parameter p vec The driving distance value corresponding to the nearest point is calculated according to the table relationship:
s nearest ＝interp1D(p vec ,s vec ,p nearest )
wherein, the interp1D represents a one-dimensional linear interpolation function.
In the step 7, the distance s is updated by the following equation p (k) Corresponding parameter value p p (k)：
p p (k)＝interp1D(s vec ,p vec ,s p (k))。
In the step 8, the discrete step index number k and the subscript p are omitted, and the reference path point p is updated p (k) The calculation equation of the corresponding pose and curvature related information is derived as follows:
the 0 to 4 derivative of the coordinate values (x, y) with respect to the parameter p is in turn:
calculating the tangential direction:
h＝arctan2(y′,x′)
and (3) curvature calculation:
first derivative calculation of curvature with respect to the path:
second derivative calculation of curvature with respect to distance:
in step 9, the deduction algorithm is as follows:
the zero-order pose of the tractor body comprises a tractor mass center position coordinate (x) f (k),y f (k) And tractor heading angle h) f (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein L is p The longitudinal distance between the tractor centroid position and the planning reference point is obtained;
the first-order kinematic quantity of the tractor body comprises a component (v) of the position and the speed of the mass center of the tractor in the direction of a self-vehicle coordinate system xf (k),v yf (k) ) and yaw rate ω f (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
the second-order kinematic quantity of the tractor body comprises a component (a) of the acceleration of the position of the center of mass of the tractor in the direction of the coordinate system of the tractor xf (k),a yf (k) And yaw angular acceleration a) f (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein the content of the first and second substances,
and
respectively, the rate of change of longitudinal and lateral velocity of the tractor's center of mass.
In step 10, the estimation process is as follows:
assuming that the circle center of the circular motion of the whole vehicle is positioned near the intersection point of the vertical lines at the mass centers of the front and rear vehicle bodies, the hinge angle theta is estimated according to the principle as follows:
θ＝-(arctan(L a c P )+arctan(L b c P ))
wherein the articulation angle theta is the angle at which the trailer rotates about the articulation angle with respect to the tractor, L a And L b The distances from the tractor mass center and the trailer mass center to the hinge point are respectively;
the first order rate of change of articulation angle is calculated as follows:
the second order rate of change of the articulation angle is calculated as follows:
in the step 11, the deduction process is as follows:
the zero-order pose of the trailer body comprises a trailer barycenter position coordinate (x) r (k),y r (k) ) and trailer heading angle h r (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
the first-order kinematic quantity of the trailer body comprises a component (v) of the position and the speed of the center of mass of the trailer in the direction of a coordinate system of the trailer xr (k),v yr (k) ) and yaw angular velocity ω r (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
the second-order kinematic quantity of the trailer body comprises a component (a) of the acceleration of the trailer center of mass position in the direction of the trailer coordinate system xr (k),a yr (k) ) and trailer yaw angular acceleration a r (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein, the first and the second end of the pipe are connected with each other,
and
respectively, the rate of change of the trailer's center of mass longitudinal and lateral velocities.
Compared with the prior art, the invention has the following advantages:
(1) The invention adopts a space trajectory + real-time expected acceleration mode to carry out perfect deduction, and the planning track deduction method provided by the invention can simultaneously realize perfect deduction of zero-order pose and first-order and second-order motion information of the trailer body and the tractor body. Under the steering driving condition, the invention can reasonably show the pose difference of the front and rear vehicle bodies, and is convenient to realize accurate and credible evaluation of the planning track result.
(2) The invention does not rely on the deduction of the planning track to express the complete space-time planning track as the input, can perfectly deduce the incomplete planning track expressed by the way of 'space trajectory + real-time expected acceleration' in the actual automatic driving of the commercial vehicle, and has strong practicability.
(3) The method for estimating the articulation angle is based on the assumption that the circular motion center of the whole vehicle is positioned near the intersection point of the vertical lines at the mass centers of the front and rear vehicle bodies, does not depend on the number and the spatial arrangement mode of trailer axles, does not limit the driving and steering modes of a traction vehicle body, and has strong universality for common commercial articulated vehicles.
Drawings
The invention will be further described with reference to the accompanying drawings.
Fig. 1 is a flowchart of a perfect deduction method for an existing automatic driving planning trajectory.
Fig. 2 is a schematic view of a commercial articulated vehicle according to the invention.
Fig. 3 is a flow chart of a perfect deduction of the planned trajectory for automatic driving of a commercial articulated vehicle according to the invention.
Fig. 4 is a schematic diagram of a "borrowing avoidance" planned space trajectory line in the present invention.
FIG. 5 is a schematic diagram of a real-time expected acceleration command curve according to the present invention.
Fig. 6 is a schematic representation of the deductive result of the articulation angle of the present invention.
Fig. 7 is a schematic representation of the deduction of the first order rate of change of articulation angle in the present invention.
FIG. 8 is a diagram illustrating the deductive result of the second order rate of change of the articulation angle in the present invention.
FIG. 9 is a schematic diagram of a deduction result of the front and rear body mass center locus in the present invention.
FIG. 10 is a schematic diagram showing a deduction of the front and rear body orientation angles in the present invention.
FIG. 11 is a schematic diagram showing the deduction of the longitudinal speed of the center of mass of the front and rear bodies according to the present invention.
FIG. 12 is a schematic diagram showing the deduction of the transverse velocity of the center of mass of the front and rear bodies according to the present invention.
FIG. 13 is a schematic diagram showing a deduction result of the yaw rate of the front and rear bodies in the present invention.
FIG. 14 is a schematic diagram showing the deduction of the longitudinal acceleration of the front and rear vehicle body center of mass in the present invention.
FIG. 15 is a schematic diagram showing the deduction of the lateral acceleration of the center of mass of the front and rear bodies according to the present invention.
FIG. 16 is a schematic diagram showing the deduction of the yaw angular acceleration of the front and rear vehicle bodies in the present invention.
FIG. 17 is a schematic view of a front and rear body form contour footprint in accordance with the present invention.
Detailed Description
The configuration of the commercial articulated vehicle is shown in figure 2, and the body of the articulated vehicle consists of a tractor and a trailer which are connected through a plane articulated mechanism which comprises five shafts. The method specifically comprises the following steps: the tractor is provided with a front axle and a rear axle, and the first axle and the second axle are agreed in sequence by numbering, wherein the first axle is a steering axle, and the second axle is a driving axle; the axle contained in the trailer body is a driven non-steering axle, the invention is suitable for the trailer body with any number of axles, and the arrangement mode among the axles of the trailer is not limited, as an example, the trailer body shown in figure 2 has 3 axles, and the appointed numbers are a third axle, a fourth axle and a fifth axle in sequence.
The specific expression mode of the planned trajectory defined by the mode of 'space trajectory + real-time expected acceleration' is as follows: in each control execution period, the planning module sends out expected space trajectories (x (p), y (p)) of the vehicle body planning reference point and expected acceleration information a at the current moment in a mode of 4 times of Bessel fitted curve trg (t) of (d). The body planning reference point is generally chosen as the center point of the second axis, the coordinates (x, y) of the desired spatial trajectory are described in the direction of the vehicle's own coordinate system, and the parameter p is [0,1]]The mathematical equation of the desired spatial trajectory is as follows:
wherein, a i ,b i (i =0,1,2,3,4) is a fitting coefficient, and a set of determined fitting coefficients uniquely determines a spatial trajectory in space. The planning module sends an expected acceleration command a to the control module in real time according to the current feedback vehicle speed and the planning driving target trg (t) of (d). The update period of the spatial trajectory and the real-time desired acceleration is no more than 100ms. The planned trajectory expressed by the "spatial trajectory + real-time desired acceleration" can be expressed by 11 parameters, namely a i ,b i (i =0,1,2,3,4) and a trg (t)。
The perfect deduction method for the planned track is suitable for the planned track defined by the commercial articulated vehicle and a mode of 'space trajectory + real-time expected acceleration'. The deduction result should cover the posture and motion information from zero order to second order of the trailer body and the tractor body, and provide input for deduction of an automatic driving scene, result visualization and power performance evaluation. The specific deduction steps of the automatic driving planning track provided by the invention are shown in fig. 3, and the process of perfectly deducing the planning track is provided.
In each deduction cycle, the specific steps are as follows:
wherein k is a discrete time step number;
the space trajectory parameter equation under the global coordinate system obtained after conversion is as follows:
wherein i is the number of discrete sampling points, x i Is the x coordinate, y, of the ith discrete sample point i Is the y coordinate, s, of the ith discrete sample point 0 As a distance value of the start of the reference path, s i-1 Is the distance coordinate, x, of the i-1 th discrete sample point i-1 X coordinate, y for i-1 discrete sample points i-1 Y being the i-1 th discrete sample pointCoordinates, p vec For a discrete sequence of parameters, s vec Is a discrete sequence of travel distances.
Sampling to obtain a distance discrete sequence s vec Is about a parameter discrete sequence p vec Monotonically increasing table of (c), while p vec Is also related to s vec The two are in one-to-one correspondence.
The spatial trajectory sampling method in the step 3 establishes the corresponding relation between the position parameters of each point of the spatial trajectory and the trajectory distance, combines dichotomy search and linear interpolation, facilitates quick indexing between the two, and constitutes an important premise of the step 4 and the step 7.
Then the driving distance s constructed by the step 3 vec And parameter p vec The driving distance value s corresponding to the nearest point is calculated by adopting a binary index method and a linear interpolation mode nearest ：
s nearest ＝interp1D(p vec ,s vec ,p nearest )
Wherein, the interp1D represents a one-dimensional linear interpolation function.
v p (k)＝v p (k-1)+T s a trg (k)
wherein, T s The deduced time step for the trajectory, i.e. the update period of the planned trajectory, does not exceed 100ms.
and 5 and 6, deducing the driving speed and the trajectory distance based on the quadratic integral of the real-time expected acceleration, and realizing the association between the trajectory described by the space domain and the vehicle motion described by the time domain, which is a key step for perfectly deducing the planned trajectory described in the mode of 'space trajectory + real-time expected acceleration'.
p p (k)＝interp1D(s vec ,p vec ,s p (k))。
the 0 to 4 derivative of the coordinate values (x, y) with respect to the parameter p is in turn:
wherein x 'is the first derivative of x with respect to the parameter p, y' is the first derivative of y with respect to the parameter p, x "is the second derivative of x with respect to the parameter p, y" is the second derivative of y with respect to the parameter p, x '"is the third derivative of x with respect to the parameter p, x'" is the third derivative of y with respect to the parameter p (4) To consider the fourth derivative, y, of the parameter p (4) A fourth derivative of y with respect to parameter p;
calculating the tangential direction:
h＝arctan2(y′,x′)
wherein h is a tangential direction angle;
and (3) curvature calculation:
wherein s 'is a first derivative of the distance coordinate s with respect to the parameter p, h' is a first derivative of the tangential direction angle h with respect to the parameter p, and c is a road curvature;
first derivative calculation of curvature with respect to the path:
wherein s "" is a second derivative of the distance coordinate s with respect to the parameter p, h "" is a second derivative of the tangential direction angle h with respect to the parameter p, c' is a first derivative of the road curvature c with respect to the parameter p, and dc/ds is a first derivative of the road curvature c with respect to the distance s;
second derivative calculation of curvature with respect to distance:
where m is an intermediate variable, introduced to simplify the formula, s 'is the third derivative of the distance coordinate s with respect to the parameter p, h' is the third derivative of the tangential direction angle h with respect to the parameter p, c 'is the second derivative of the road curvature c with respect to the parameter p, (dc/ds)' is the first derivative of dc/ds with respect to the parameter p, d 2 c/ds 2 Is the second derivative of the road curvature c with respect to the distance coordinate s.
the zero-order pose of the tractor body comprises a tractor centroid position coordinate (x) f (k),y f (k) And tractor heading angle h f (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein h is f Is the orientation angle of the tractor, h p Is the tangential angle, x f 、y f Is a coordinate value of the position of the center of mass of the tractor, x p ，y p Planning reference point coordinate values, L, for the vehicle body p For the longitudinal distance between the tractor centroid position and the planning reference point, L when the centroid position is before the planning reference point p Take a positive value.
The first-order kinematic quantity of the tractor body comprises the speed of the position and the mass center of the tractor in the self-vehicle seatComponent (v) in the direction of the coordinate system xf (k),v yf (k) ) and yaw angular velocity ω f (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein, ω is f Is yaw rate, v p Planning the travel speed of the reference point for the vehicle body, c p Is curvature, v xf ，v yf The component value of the tractor centroid position speed in the direction of the self-vehicle coordinate system is shown.
The second-order kinematic quantity of the tractor body comprises a component (a) of the acceleration of the position of the center of mass of the tractor in the direction of the coordinate system of the tractor xf (k),a yf (k) And yaw angular acceleration a) f (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein alpha is f As yaw angular acceleration, a trg In order to expect the acceleration in real time,
the first derivative of the road curvature c at point P with respect to the distance coordinate s,
and
the rate of change of longitudinal and transverse speed of the mass center of the tractor, a xf ，a yf The component values of the acceleration of the center of mass position of the tractor in the direction of the coordinate system of the tractor are shown.
And 9, calculating zero-order pose, first-order kinematic quantity and second-order kinematic quantity at the position of the mass center of the traction vehicle body based on the vehicle body planning reference point motion information updated in the step 5 and the pose geometric information calculated from the space trajectory reference point obtained in the step 8, wherein the calculation is one of main presentation of deduction results.
assuming that the circle center of the circular motion of the whole vehicle is positioned near the intersection point of the vertical lines at the mass centers of the front and rear vehicle bodies, the hinge angle theta is estimated according to the principle as follows:
θ＝-(arctan(L a c P )+arctan(L b c P ))
wherein the articulation angle theta is defined as the angle of rotation of the trailer about the articulation angle relative to the tractor, the counterclockwise rotation is positive, and L a And L b The distances from the tractor mass center and the trailer mass center to the hinge point are respectively.
First order rate of change of articulation angle
The calculation is as follows:
second order rate of change of articulation angle
The calculation is as follows:
wherein the content of the first and second substances,
the second derivative of the road curvature c at point P with respect to the distance s.
And step 11, perfectly deducing the zero-order pose, the first-order kinematic quantity and the second-order kinematic quantity related to the position of the mass center of the trailer body according to the deduced motion information of each order of the traction vehicle body in the step 9 and the deduced motion information of each order of the hinge angle in the step 10, wherein the deduction process comprises the following steps:
the zero-order pose of the trailer body comprises a trailer mass center position coordinate (x) r (k),y r (k) ) and trailer heading angle h r (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein h is r For trailer steering angle, x r ，y r The zero-order pose of the trailer body comprises a coordinate value of the position of the mass center of the trailer;
the first-order kinematic quantity of the trailer body comprises a component (v) of the position and the speed of the center of mass of the trailer in the direction of a coordinate system of the trailer xr (k),v yr (k) ) and yaw rate ω r (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein, ω is r Is yaw rate, v xr ，v yr The component values of the trailer mass center position speed in the direction of the trailer coordinate system are obtained;
the second-order kinematic quantity of the trailer body comprises a component (a) of the acceleration of the trailer center of mass position in the direction of the trailer coordinate system xr (k),a yr (k) ) and trailer yaw angular acceleration a r (k) The discrete step index k is omitted, and the deduction is as follows:
wherein, the first and the second end of the pipe are connected with each other,
is the time rate of change of the trailer body yaw rate,
is the time rate of change of the yaw rate of the tractor body,
and
respectively the rate of change of the longitudinal and lateral velocities of the trailer's center of mass,
respectively the rate of change of the longitudinal speed and the rate of change of the transverse speed of the mass centre of the tractor body, a xr ，a yr The component values of the acceleration of the center of mass position of the trailer in the direction of the coordinate system of the trailer are taken as the component values,
respectively the rate of change of the longitudinal speed and the rate of change of the transverse speed, omega, of the center of mass of the trailer body r The yaw rate of the trailer body.
And step 11, calculating the zero-order pose, the first-order kinematic quantity and the second-order kinematic quantity at the position of the center of mass of the trailer body according to the deduced motion information of each order of the traction vehicle body in the step 9 and the deduced motion information of each order of the articulation angle in the step 10, and is one of the main presentation of the deductive result.
And 12, inputting the kinematic quantities of all steps of the tractor and the trailer obtained by deduction in the steps 9 and 11 into a traffic scene deduction program, wherein the traffic scene deduction program combines dynamic scene files and static map information, each traffic participant generates an interactive behavior with the articulated tractor, and the whole process data is finally input into a visualization and evaluation module for automatic driving algorithm development and analysis and evaluation of testers.
And finally, when the deduction termination time condition is established, circularly exiting, and finishing the automatic driving planning track perfect deduction process of the commercial articulated vehicle.
Example one
In order to demonstrate the effect of the method for perfectly deducing the planned trajectory of the commercial articulated vehicle, the present embodiment uses an actual commercial articulated vehicle shown in fig. 2 as a prototype for demonstration, and the geometric parameter values of the vehicle used in the deduction device are shown in table 1.
Table 1 articulated vehicle geometry parameter examples
For convenient demonstration, a group of space trajectories representing the 'lane borrowing avoiding' working condition of the vehicle are selected as the input of the deduction device, the initial vehicle speed is set to be 10km/h, and meanwhile, the expected acceleration is adjusted in real time through a virtual speed planner, so that the articulated vehicle is guaranteed to finish rapid 'lane borrowing avoiding' driving at the vehicle speed according with physical constraints and safety requirements. The corresponding fitting coefficient of the 'borrowing and avoiding' space trajectory is shown in the table 2, the corresponding space trajectory is shown in the figure 4, and the expected acceleration command curve output by the virtual speed planner in the whole deduction process is shown in the figure 5.
TABLE 2 "borrow avoiding" space trajectory fitting coefficients
The geometric parameters of the prototype vehicle and the automatic driving planning track of the commercial articulated vehicle defined by the space trajectory and real-time expected acceleration are determined by the setting, and the information of each order of motion process of the tractor, the trailer and the articulation angle can be obtained by deduction of the planning track perfect deduction device provided by the invention. Fig. 6, 7 and 8 show the deduction of the articulation angle and its first and second order rates of change, respectively. Figures 9 to 17 fully illustrate the deduction of the various stages of motion information of the tractor and trailer. Wherein, fig. 9 comparatively shows the plane motion trajectories corresponding to the tractor centroid, the trailer centroid and the planning reference point; FIG. 10 is a comparative illustration of the change in heading angle of two vehicle bodies; FIG. 11 shows the longitudinal speed variation of the mass center of the two car bodies in comparison; FIG. 12 shows the variation of the lateral velocity of the center of mass of two vehicle bodies in comparison; FIG. 13 shows a comparison of yaw rate changes of two vehicle bodies; FIG. 14 is a graph showing the change in longitudinal acceleration of the center of mass of two vehicle bodies in comparison; FIG. 15 shows the variation of the lateral acceleration of the mass center of the two vehicle bodies in comparison; FIG. 16 shows the yaw acceleration change of two vehicle bodies in comparison; fig. 17 shows the footprint of the rectangular profile of the tractor and trailer bodies driving over a flat surface.
It can be seen from the combination of fig. 6-17 that the method for perfectly deducing the planned track of the commercial articulated vehicle can perfectly deduct the motion tracks of the tractor body and the trailer body, the deduction result can meet the 'borrowing and avoiding' expectation of the planned track, and the method conforms to the articulation constraint relationship between the front and rear vehicle bodies. The method for perfectly deducing the planned track can provide a powerful basis for visualization and power/performance evaluation of the automatic driving planned track of the commercial articulated vehicle.
In addition to the above embodiments, the present invention may have other embodiments. All technical solutions formed by adopting equivalent substitutions or equivalent transformations fall within the protection scope of the claims of the present invention.
Claims (10)
1. A perfect deduction method for a commercial articulated vehicle automatic driving planning track is characterized by comprising the following steps:
step 1, passing a space trajectory fitting coefficient a through an automatic driving planning trajectory of a commercial transfer vehicle i ,b i (i =0,1,2,3,4) and a real-time desired acceleration a trg (k) To represent;
step 2, according to the actual orientation angle h of the tractor in the previous step f (k-1) and body planning reference point coordinates x p (k-1),y p (k-1) fitting the space trajectory fitting coefficient a under the own vehicle coordinate system i ,b i (i =0,1,2,3,4) into a fitting coefficient α in the global coordinate system i ,β i (i＝0,1,2,3,4)；
Step 3, sampling the space trajectory, and carrying out N on p within the range that p is more than or equal to 0 and less than or equal to 1 s Equal sampling, and constructing sampling points p by accumulating Euclidean distances between sampling points i Distance s from the vehicle i The mathematical expression of the constructed relationship is as follows:
step 4, searching the coordinate x of the actual planning reference point of the vehicle body in the previous step p (k-1),y p (k-1) location of closest point in the spatial trajectory, distance traveled s constructed by step 3 vec And parameter p vec The table relationship between the two points is obtained, and the driving distance value s corresponding to the nearest point is obtained nearest ；
Step 5, according to the actual running speed v of the vehicle body planning reference point in the previous step p (k-1) and the expected acceleration issued by the planning track, and the running speed v of the next vehicle body planning reference point p (k) Performing integral deduction, wherein the formula is as follows:
v p (k)＝v p (k-1)+T s a trg (k)
wherein, T s A time step for the trajectory deduction;
step 6, obtaining the shortest distance s of the last moment according to the step 4 nearest And step 5 updating the running speed (v) at the adjacent moment p (k-1),v p (k) Estimate the distance s to the vehicle at the next time in the ideal case p (k) The estimation equation is as follows:
step 7, updating the obtained s according to the step 6 p (k) Parameter p constructed in connection with step 3 vec Distance s from the vehicle vec The table relationship between them, update the distance s p (k) Corresponding ginsengNumber p p (k)；
Step 8, calculating the vehicle updating point parameter p according to the step 7 p (k) And (3) calculating an updated reference path point p by combining the space trajectory equation of the planned path in the global coordinate system obtained in the step (2) p (k) Corresponding pose and curvature related information including coordinate value x p (k),y p (k) Tangential angle h p (k) Curvature c p (k) And first order rate of change of curvature with respect to path
And second order rate of change
Step 9, updating the vehicle body planning reference point speed v according to the step 5 p (k) And a desired acceleration a trg (k) Performing perfect deduction on the zero-order pose, the first-order kinematic quantity and the second-order kinematic quantity related to the position of the mass center of the traction vehicle body by combining the pose geometric information at the vehicle planning path updating point calculated in the step 8;
step 10, updating the curvature c according to the current planning reference point p (k) In combination with the current planned reference point velocity v p (k) And a desired acceleration a trg (k) Estimating the articulation angle and the first-order and second-order change rates thereof;
step 11, performing perfect deduction on the zero-order pose, the first-order kinematic quantity and the second-order kinematic quantity related to the position of the mass center of the trailer body according to the deduced motion information of each order of the traction vehicle body in the step 9 and the deduced motion information of each order of the articulation angle in the step 10;
and step 12, inputting the kinematics quantities of each step of the tractor and the trailer obtained by deduction in the step 9 and the step 11 into a traffic scene deduction program, and inputting the data of the whole deduction process into a visualization and evaluation module for automatic driving algorithm development and analysis and evaluation of testers.
2. The method for perfectly deducing the planned trajectory for automatic driving of a commercial articulated vehicle according to claim 1, wherein in the step 1, the planned trajectory is expressed in a specific way as follows: in each control execution period, the planning module sends out expected space trajectories (x (p), y (p)) of the vehicle body planning reference point and expected acceleration information at the current moment; the vehicle body planning reference point is generally selected as a central point of a driving shaft of the tractor, the coordinates (x, y) of the expected space trajectory are described in the direction of a coordinate system of the vehicle, the parameter p is continuously taken between [0,1], and a mathematical equation of the expected space trajectory is as follows:
wherein, a i ,b i (i =0,1,2,3,4) is a fitting coefficient, and the planning module sends an expected acceleration command a to the control module in real time according to the current feedback vehicle speed and the planned driving target trg (k)。
3. The method for perfectly deducing the planned trajectory for automatic driving of a commercial articulated vehicle according to claim 1, wherein in the step 2, the transformation equation is as follows:
the space trajectory parameter equation under the global coordinate system obtained after conversion is as follows:
4. the method for perfectly deducing the planned trajectory for automatic driving of a commercial articulated vehicle according to claim 1, wherein in step 3, the discrete sequence of distances s obtained by sampling vec Is about a parametric discrete sequence p vec Monotonically increasing table of, while the parameter is a discrete sequence p vec Also with respect to the distance discrete sequence s vec The two are in one-to-one correspondence.
5. The method of claim 1, wherein in step 4, the nearest corresponding parameter p is first obtained by solving the following mathematical optimization problem nearest :
Then the driving distance s constructed by the step 3 vec And parameter p vec The driving distance value corresponding to the nearest point is calculated according to the table relationship:
s nearest ＝interp1D(p vec ,s vec ,p nearest )
wherein, the interp1D represents a one-dimensional linear interpolation function.
6. The method of claim 1, wherein in step 7, the distance s is updated by the following formula p (k) Corresponding parameter value p p (k)：
p p (k)＝interp1D(s vec ,p vec ,s p (k))。
7. The method of claim 1, wherein in step 8, the discrete step index k and the subscript p are omitted, and the updated reference path point p is used as the reference path point p p (k) The calculation equation of the corresponding pose and curvature related information is derived as follows:
the 0 to 4 derivative of the coordinate values (x, y) with respect to the parameter p is in turn:
calculating the tangential direction:
h＝arctan2(y′,x′)
and (3) curvature calculation:
first derivative calculation of curvature with respect to the path:
second derivative calculation of curvature with respect to distance:
8. the method of claim 1, wherein in step 9, the deductive algorithm is as follows:
the zero-order pose of the tractor body comprises a tractor mass center position coordinate (x) f (k),y f (k) And tractor heading angle h) f (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
wherein L is p The longitudinal distance between the tractor centroid position and the planning reference point is obtained;
the first-order kinematic quantity of the traction vehicle body comprises the speed of the position and the mass center of the tractor in the self vehicleComponent (v) in the direction of the coordinate system xf (k),v yf (k) ) and yaw angular velocity ω f (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
the second-order kinematic quantity of the tractor body comprises a component (a) of the acceleration of the position of the center of mass of the tractor in the direction of the coordinate system of the tractor xf (k),a yf (k) And yaw angular acceleration a) f (k) The discrete step index k is omitted, and the deduction is as follows:
wherein the content of the first and second substances,
and
respectively, the rate of change of longitudinal and lateral velocity of the tractor's center of mass.
9. The method for perfectly deducing the planned trajectory for automatic driving of a commercial articulated vehicle according to claim 1, wherein in said step 10, the estimation procedure is as follows:
assuming that the circle center of the circular motion of the whole vehicle is positioned near the intersection point of the vertical lines at the mass centers of the front vehicle body and the rear vehicle body, and estimating the hinge angle theta according to the principle as follows:
θ＝-(arctan(L a c P )+arctan(L b c P ))
wherein the articulation angle theta is the angle at which the trailer rotates about the articulation angle with respect to the tractor, L a And L b The distances from the tractor mass center and the trailer mass center to the hinge point are respectively;
the first order rate of change of articulation angle is calculated as follows:
the second order rate of change of articulation angle is calculated as follows:
10. the method for perfectly deducing the trajectory for automatic driving planning of commercial articulated vehicles according to claim 1, wherein in said step 11, the deduction process is as follows:
the zero-order pose of the trailer body comprises a trailer mass center position coordinate (x) r (k),y r (k) ) and trailer heading angle h r (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
the first-order kinematic quantity of the trailer body comprises a component (v) of the position and the speed of the center of mass of the trailer in the direction of a coordinate system of the trailer xr (k),v yr (k) ) and yaw angular velocity ω r (k) The discrete step index k is omitted and is calculated in a deductive manner as follows:
the second-order kinematic quantity of the trailer body comprises a component (a) of the acceleration of the trailer center of mass position in the direction of the trailer coordinate system xr (k),a yr (k) And trailer yaw angular acceleration a) r (k) The discrete step index k is omitted, and the deduction is as follows:
wherein, the first and the second end of the pipe are connected with each other,
and
respectively, the rate of change of the trailer's center of mass longitudinal and lateral velocities.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211385064.9A CN115717898A (en) | 2022-11-07 | 2022-11-07 | Automatic driving planning trajectory perfect deduction method for commercial articulated vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211385064.9A CN115717898A (en) | 2022-11-07 | 2022-11-07 | Automatic driving planning trajectory perfect deduction method for commercial articulated vehicle |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115717898A true CN115717898A (en) | 2023-02-28 |
Family
ID=85254890
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211385064.9A Pending CN115717898A (en) | 2022-11-07 | 2022-11-07 | Automatic driving planning trajectory perfect deduction method for commercial articulated vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115717898A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117272693A (en) * | 2023-11-21 | 2023-12-22 | 上海友道智途科技有限公司 | Method for estimating articulation angle of commercial articulated vehicle in planned track |
-
2022
- 2022-11-07 CN CN202211385064.9A patent/CN115717898A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117272693A (en) * | 2023-11-21 | 2023-12-22 | 上海友道智途科技有限公司 | Method for estimating articulation angle of commercial articulated vehicle in planned track |
| CN117272693B (en) * | 2023-11-21 | 2024-03-01 | 上海友道智途科技有限公司 | Method for estimating articulation angle of commercial articulated vehicle in planned track |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Li et al. | Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles | |
| Vázquez et al. | Optimization-based hierarchical motion planning for autonomous racing | |
| CN103121451B (en) | A kind of detour changes the tracking and controlling method of track | |
| Pérez et al. | Trajectory generator for autonomous vehicles in urban environments | |
| CN108646763A (en) | A kind of autonomous driving trace tracking and controlling method | |
| Zhang et al. | Automatic vehicle parallel parking design using fifth degree polynomial path planning | |
| Kapania | Trajectory planning and control for an autonomous race vehicle | |
| Peicheng et al. | Intelligent vehicle path tracking control based on improved MPC and hybrid PID | |
| CN113433947A (en) | Intersection trajectory planning and control method based on obstacle vehicle estimation and prediction | |
| Kegelman | Learning from professional race car drivers to make automated vehicles safer | |
| CN115717898A (en) | Automatic driving planning trajectory perfect deduction method for commercial articulated vehicle | |
| Zhang et al. | Smooth path and velocity planning under 3D path constraints for car-like vehicles | |
| CN114942642A (en) | Unmanned automobile track planning method | |
| Martin et al. | Design and simulation of control strategies for trajectory tracking in an autonomous ground vehicle | |
| Filip | Trajectory tracking for autonomous vehicles | |
| CN110654386B (en) | Cooperative cruise longitudinal and transverse comprehensive control method for multiple intelligent electric vehicles under curve | |
| CN110723207B (en) | Intelligent automobile model prediction steering controller based on model reconstruction and control method thereof | |
| Kone | Lateral and longitudinal control of an autonomous racing vehicle. | |
| CN110562245A (en) | Automatic parking space searching path following control method and system, storage medium and vehicle | |
| CN114896694A (en) | Path tracking control method based on two-point preview | |
| Björnberg | Shared control for vehicle teleoperation with a virtual environment interface | |
| CN115079699A (en) | Model prediction control-based motion planning method for man-machine co-driving automobile | |
| CN111857112B (en) | Automobile local path planning method and electronic equipment | |
| Shufeng et al. | Lane level turning trajectory tracking of intelligent vehicle based on drivers' manipulate habits | |
| Dai et al. | Online path tracking and motion optimization of a 4WS4WD vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |