CN117272693B - Method for estimating articulation angle of commercial articulated vehicle in planned track - Google Patents
Method for estimating articulation angle of commercial articulated vehicle in planned track Download PDFInfo
- Publication number
- CN117272693B CN117272693B CN202311549019.7A CN202311549019A CN117272693B CN 117272693 B CN117272693 B CN 117272693B CN 202311549019 A CN202311549019 A CN 202311549019A CN 117272693 B CN117272693 B CN 117272693B
- Authority
- CN
- China
- Prior art keywords
- tractor
- angle
- order
- planning
- steering
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 58
- 230000008859 change Effects 0.000 claims abstract description 72
- 230000001133 acceleration Effects 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 11
- 230000014509 gene expression Effects 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 238000004088 simulation Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 230000002159 abnormal effect Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000007796 conventional method Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000005299 abrasion Methods 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 239000004575 stone Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/10—Geometric CAD
- G06F30/15—Vehicle, aircraft or watercraft design
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2111/00—Details relating to CAD techniques
- G06F2111/04—Constraint-based CAD
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Geometry (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
The invention discloses a method for estimating a hinging angle of a commercial hinging vehicle in a planned track, which is used for determining the position and the orientation of a tractor based on geometric information in the planned track line, extracting curvature and each-order spatial derivative information in the planned track, and laying a data foundation for estimating the subsequent hinging angle and each-order change rate; determining a geometric steering center according to curvature information, and reasonably estimating the steering angle of a front axle of the tractor and the time change rate of the steering angle according to the movement information and the wheelbase of the tractor; based on the assumption of minimizing equivalent axis tire kinematic interference, geometrically deriving and solving the hinge angle and the information of each derivative thereof, and limiting the locking of the hinge angle; meanwhile, a space distance constant is introduced, and a first-order hysteresis system is constructed in a time domain in combination with the driving speed of the tractor to act on the geometrical theoretical values of the hinge angle and the information of each derivative thereof, so that the dynamic change characteristic of the estimation result of the hinge angle is closer to reality.
Description
Technical Field
The invention belongs to the technical field of automatic driving, relates to automatic driving simulation, and particularly relates to a method for estimating a hinge angle of a commercial hinge vehicle in a planned track.
Background
In the field of logistics, commercial articulated vehicles are the preferred carrier for container land transportation due to their high load carrying capacity and flexible mobility. In recent years, with the continuous rise of the demand for low-cost unmanned transportation, the automatic driving technology of commercial articulated vehicles has been vigorously developed. The dual body articulation feature of commercial articulated vehicles presents new challenges to the perception, planning and control technology of autopilot, as opposed to the autopilot technology of general passenger vehicles. The high-precision hinge angle estimation result can remarkably improve the accuracy of the automatic driving perception result of the commercial hinge vehicle, the safety of a planned track and the accuracy of motion control.
In the existing method for estimating the articulation angle of a commercial articulated vehicle in a planned track, it is generally assumed that a tractor planned reference point and a trailer equivalent axis center point are simultaneously positioned on the planned track in the running process of the vehicle, and then the articulation angle is estimated by combining the geometric information of the planned track line at the tractor planned reference point and the size information of the trailer, and the specific estimation steps are as follows:
step 1: firstly, placing a tractor planning reference point P (which can be selected as a center point of a tractor rear axle) on a planning track, determining the position of the tractor in the planning track, and simultaneously determining the head direction of the tractor according to the tangential direction of the point P in the planning track;
step 2: on the basis of determining the traction vehicle position and posture, taking a hinge point A as a circle center, taking a trailer equivalent wheelbase T as a radius as a circle, and setting the circle to be intersected with a planning track line at a point Q, wherein the point Q is the center point of the trailer equivalent wheelbase, so that the position and the orientation of the trailer in the planning track are uniquely determined by the connecting line of the hinge point A and the center point Q of the trailer equivalent wheelbase;
step 3: according to the head direction determined in the step 1 and the trailer direction determined in the step 2, the two directions are differenced, namely the estimated value of the hinge angle in the planned track, and the hinge angle。
However, the above estimation method has the following disadvantages:
1. the existing method lacks theoretical basis when assuming that a tractor planning reference point and a trailer equivalent shaft center point are simultaneously positioned on a planning track, when an actual articulated vehicle runs in a steering way, the tractor wheel track and the trailer wheel track are not coincident, and particularly when in large-curvature steering, the assumption condition leads to larger estimation error;
2. in the implementation process of the existing method, the condition that a plurality of coincident points exist at the center point of the equivalent axis of the trailer in the planned track possibly exists, and particularly, certain difficulty exists in reasonably choosing and rejecting a plurality of solutions under the condition that the curvature of the planned track line and the size of the trailer are large;
3. the rationality of the trailer corner estimation result of the existing method has higher rationality requirement on the planned trajectory line, when the planned trajectory line has abnormal phenomena such as curvature alternation, the trailer corner also has abnormal alternation, and the response situation of the planned trajectory line to the abnormal planned trajectory line in the actual vehicle driving process is greatly different;
4. the existing method can only estimate the hinge angle, and cannot estimate the first-order and second-order change rates, namely the hinge angular speed and the hinge angular acceleration, however, the hinge angular speed and the hinge angular acceleration have important significance for a prediction algorithm of the pose of the hinged vehicle.
Disclosure of Invention
In view of the above problems, a main object of the present invention is to design a method for estimating a hinge angle of a commercial hinge vehicle in a planned trajectory, which solves the technical problem of a larger error of an estimation result based on the assumption of minimizing equivalent axis tire kinematic interference, and estimates first-order and second-order change rates of the hinge angle at the same time, so that the estimation result can support prediction of high-order information such as speed, acceleration, angular speed, angular acceleration, and the like of a hinge double-vehicle body.
The invention adopts the following technical scheme for realizing the purposes:
a method of estimating a articulation angle of a commercial articulated vehicle in a planned trajectory, comprising the steps of:
step 1: taking a planning track line and a planning reference point as inputs, placing the planning reference point on the planning track line, and determining the position coordinates and the orientation of the tractor through the coordinates and the tangential direction of the planning reference point;
step 2: acquiring curvature information of a planning reference point in a planning track line and spatial change rate information of the planning reference point;
step 3: determining a geometric steering radius according to curvature information in a planned track, positioning a geometric steering center point on a vertical line in a rear wheel of the tractor, and estimating a steering angle and a time change rate of the front wheel of the tractor by combining the driving speed, acceleration information and front and rear wheel base of the tractor based on the curvature and the space change rate obtained in the step 2 to enable a connecting line from the center of the front wheel of the tractor to the geometric steering center to be perpendicular to a longitudinal center plane of the front wheel of the tractor;
step 4: based on the assumption of the minimized equivalent axle tire kinematic interference, connecting lines from the front wheel center point of the tractor, the rear axle center point of the tractor and the trailer equivalent axle center point to the geometric steering center are perpendicular to the longitudinal center plane of each wheel, and the steering angle of the front wheel of the tractor and the time change rate thereof obtained in the step 3 are combined to solve the hinging angle and the information of each derivative thereof;
step 5: and (3) according to the longitudinal speed of the tractor, performing hysteresis processing on the articulation angle and each derivative thereof obtained in the step (4).
As a further description of the present invention, in step 1, the planning reference point is selected as the center point of the rear axle of the tractor, and the coordinates and tangential expressions of the planning reference point are as follows:
，
wherein,respectively two-dimensional plane coordinates and directions of the central point of the rear axle of the tractor,respectively two-dimensional plane coordinates and curve tangential directions of the planning reference point P in the planning track line.
As a further description of the present invention, in step 2, the spatial rate information at the planned reference point includes first-order and second-order rate information of the distance travelled by the tractor, and then the curvature of the planned reference point in the planned trajectory and the first-order and second-order rate information thereof are expressed as:
，
wherein P is a planning reference point of the tractor,for differential operation, ++>For the distance travelled by the vehicle>For planning the curvature of the reference point P in the planned trajectory, +.>，/>The first and second rates of change of curvature with respect to distance travelled,respectively planning reference points P to +.>，/>，/>Is a mapping relation of (a) to (b).
As a further description of the invention, in step 3, the geometric steering radius R of the tractor is derived by planning the curvature of the reference point, and is expressed as:
，
wherein O is a geometric steering center point on an extension line of a perpendicular bisector of a rear wheel of the tractor, and P is a planning reference point;
connecting the point O with the center point F OF the front wheel OF the tractor to enable the longitudinal center plane OF the front wheel OF the tractor to be perpendicular to the connecting line OF, obtaining the steering angle OF the front wheel OF the tractor, and solving the first-order and second-order time change rate OF the steering angle OF the front wheel OF the tractor by combining the real-time motion information OF the vehicle and the front-rear wheelbase OF the tractor, wherein the expression is as follows:
，
wherein,for the front-rear wheelbase of the tractor, +.>，/>For time (I)>For differential operation, ++>For the distance travelled by the vehicle>For the speed of the tractor>I.e. +.>For the rate of change of the distance travelled by the tractor with respect to time, < >>Tangential acceleration for tractor travel, +.>I.e. +.>For the rate of change of the speed of the tractor with respect to time, < >>The first and second order change rates of the curvature of the planning reference point P with respect to time are respectively +.>For the steering angle of the front wheels of the tractor +.>，/>The first and second order rates of change of the steering angle of the front wheels of the tractor with respect to time, respectively, arctan represents the arctan function.
As a further description of the present invention, in step 4, the tractor is fixed according to the position and orientation information of the tractor, and the articulation angle is adjusted until the connection OQ between the geometric steering center point O and the center point Q of the equivalent axis of the trailer is perpendicular to the longitudinal center axis AQ of the trailer, and then the articulation angle is expressed as:
，
wherein,for the distance of the hinge point a from the planned reference point P, i.e. the distance of the hinge point a from the center of the rear axle of the tractor, +.>For the distance of the hinge point A to the center point Q of the equivalent axle of the trailer, i.e. the equivalent trailer wheelbase, then +.>Is the hinge angle.
As a further description of the present invention, geometric equations for articulation angleAnd simultaneously deriving the two sides, and deriving first and second derivatives of the hinge angle, wherein the first and second derivatives are expressed as:
，
wherein,for the first derivative of the articulation angle, +.>Is the second derivative of the articulation angle.
As a further description of the present invention, if the hinge angle and the first and second order change rates are estimated to be true, the constraint conditions to be satisfied are:
，
if it isThe constraint is expressed as:
，
the state of the hinge angle is determined by the constraint conditions described above.
As a further description of the invention, if the steering angle of the front wheels of the tractor exceeds the upper limit of the constraint condition, the articulation angle reaches a physical limit and enters a locked state in which the articulation angle and its first and second order rate of change estimates are expressed as:
，
wherein,estimated for the articulation angle in the locked state, +.>And->The first-order and second-order change rates of the hinge angle in the locked state are respectively; sign represents a sign taking function;
and if the steering angle of the front wheels of the tractor returns to the upper limit of the constraint condition, the locking state is released.
As a further description of the present invention, in step 5, during dynamic steering of the vehicle, a first-order hysteresis process is performed for the articulation angle and its respective derivatives, expressed as:
，
wherein,is a space distance constant, ">Conversion of the spatial distance constant into a first-order system time constant in the time domain, +.>For Laplase operator, +.>For the hinge angle after the first order dynamic hysteresis process,/->，/>The first-order and second-order change rates of the hinge angle with respect to time after the first-order dynamic hysteresis processing are respectively carried out.
A vehicle comprising a tractor and a trailer and performing the above-described articulation angle estimation method.
Compared with the prior art, the invention has the technical effects that:
the invention provides a method for estimating the hinging angle of a commercial hinging vehicle in a planned track, which is based on the assumption of minimizing equivalent axis tire kinematic interference, is more in line with dynamics and kinematic characteristics of the hinging vehicle during steering and driving, the estimation result is more in line with an actual value, the hinging angle estimation value is provided with a unique solution by using the curvature at a specific track planning point and the spatial variation rate thereof as a data base, the reasonable choosing and choosing problem of a plurality of solutions possibly existing in a large-curvature alternating planned track by the existing method is avoided, a space distance constant is introduced, a geometric theoretical value of a time domain first-order hysteresis system acting on the hinging angle estimation result is constructed by combining with the speed of a tractor, the dynamic variation characteristic of the estimation result in the planned track is more in line with reality, the requirement of the estimation rationality on the curvature continuity of the planned track is reduced, and simultaneously, the first-order and second-order variation rates of the hinging angle, namely the hinging angle speed and the angular acceleration are also estimated, so that the estimation result can support the prediction of high-order information such as the speed, the acceleration, the angular speed and the angular acceleration of a hinging double-vehicle body.
Drawings
FIG. 1 is a schematic view showing the overall steps of a hinge angle estimation method according to the present invention;
FIG. 2 is a schematic view of a vehicle state according to the hinge angle estimation method of the present invention;
FIG. 3 is a table of characteristic geometric parameters of the articulated vehicle of the invention;
FIG. 4 is a schematic diagram of the planned spatial trajectory for the high speed lane change condition and the low speed U-bend steering condition of the present invention;
FIG. 5 is a table of four-order Bezier curve fitting coefficients under two conditions of the present invention;
FIG. 6 is a schematic view of the hinge angle and the estimation results of each stage under the working condition of high-speed lane change according to the present invention;
FIG. 7 is a schematic diagram comparing the estimated result of the hinge angle with the estimated result of the prior art under the high-speed lane change condition of the present invention;
FIG. 8 is a schematic diagram of the low-speed U-bend steering condition hinge angle and the estimation results of each stage;
FIG. 9 is a graph showing the comparison of the estimated result of the hinge angle under the low-speed U-bend steering condition of the present invention with the estimated result of the prior art.
Detailed Description
The invention is described in detail below with reference to the attached drawing figures:
in one embodiment of the present invention, a method for estimating the articulation angle of a commercial articulated vehicle in a planned trajectory is disclosed, and referring to fig. 1-2, the method is based on the assumption of "minimizing equivalent axis tire kinematic interference", the schematic diagram of which is shown in fig. 2, and in fig. 2, the planned reference point is selected as a center point P of a rear axle of a tractor, a front axle of the tractor is a steering axle, the rear axle is a non-steering driving axle, and the equivalent axle of a trailer is a non-steering driven axle. The assumption of "minimizing equivalent axle tire kinematic interference" is that when the articulated vehicle is traveling in the planned trajectory, the connecting lines of the front axle monorail equivalent wheel center point F of the tractor, the rear axle monorail equivalent wheel center point P of the tractor, and the trailer equivalent axle monorail equivalent wheel center point Q to the steering center point O are all perpendicular to the longitudinal center planes of the respective equivalent wheels. Under the assumption, each equivalent wheel of each shaft performs pure rolling motion around the rotation direction central point O, the comprehensive kinematic interference of the tire is minimum, and the abrasion degree of the tire is minimum. Since the actual articulated vehicle always changes the articulation angle in the direction of as little tire wear as possible during running, the articulation angle estimation result based on "minimizing equivalent axis tire kinematic interference" is close to the actual situation, especially during steady-state steering with large curvature. According to the method for estimating the hinge angle provided on the basis of the assumption, the position and the posture information of the hinged double-car body in the planned trajectory line can be estimated more accurately, a more reliable judgment basis is provided for avoiding collision with other traffic participants, and meanwhile, an automatic driving simulation developer can perform more reasonable perfect control deduction on the position and the posture of the hinged double-car body on the basis of the planned trajectory result.
More specifically, in this embodiment, based on the assumption of "minimizing equivalent axis tire kinematic interference", in combination with geometric information of a planned reference point in a planned track, real-time motion information of an articulated tractor, and feature size information of an articulated vehicle, a first-order system space distance constant for dynamic characteristics of an articulation angle is introduced, and dynamic changes of various orders of information of the articulation angle in the planned track are reasonably estimated, and specific steps are shown in fig. 1, and are disclosed as follows:
step 1: taking a planning track line and a planning reference point as inputs, placing the planning reference point on the planning track line, and determining the position coordinates and the orientation of the tractor through the coordinates and the tangential direction of the planning reference point;
step 2: acquiring curvature information of a planning reference point in a planning track line and spatial change rate information of the planning reference point;
step 3: determining a geometric steering radius according to curvature information in a planned track, positioning a geometric steering center point on a vertical line in a rear wheel of the tractor, and estimating a steering angle and a time change rate of the front wheel of the tractor by combining the driving speed, acceleration information and front and rear wheel base of the tractor based on the curvature and the space change rate obtained in the step 2 to enable a connecting line from the center of the front wheel of the tractor to the geometric steering center to be perpendicular to a longitudinal center plane of the front wheel of the tractor;
step 4: based on the assumption of the minimized equivalent axle tire kinematic interference, connecting lines from the front wheel center point of the tractor, the rear axle center point of the tractor and the trailer equivalent axle center point to the geometric steering center are perpendicular to the longitudinal center plane of each wheel, and the steering angle of the front wheel of the tractor and the time change rate thereof obtained in the step 3 are combined to solve the hinging angle and the information of each derivative thereof;
step 5: and (3) according to the longitudinal speed of the tractor, performing hysteresis processing on the articulation angle and each derivative thereof obtained in the step (4).
The steps 1 and 2 are based on geometric information in the planned trajectory, the position and the orientation of the tractor are determined, curvature and each-order spatial derivative information in the planned trajectory are extracted, and a data foundation is laid for the estimation of the subsequent hinge angle and each-order change rate of the subsequent hinge angle; step 3, determining a geometric steering center according to curvature information, reasonably estimating the steering angle of a front axle of the tractor and the time change rate of the steering angle according to the movement information and the wheelbase of the tractor, and being a key premise for implementing the subsequent hinge angle estimation; step 4, fully utilizing the assumption of minimizing equivalent axis tire kinematic interference, geometrically deriving and solving the hinge angle and the information of each derivative, and limiting the locking limit of the hinge angle; and 5, by introducing a space distance constant and combining the driving speed of the tractor, constructing a geometric theoretical value of the first-order hysteresis system acting on the hinge angle and the information of each derivative thereof in a time domain, so that the dynamic change characteristic of the estimation result of the hinge angle is closer to reality. More specifically, in this embodiment, the following is specifically described with respect to the above steps:
in step 1, a planned reference point P (selected as a central point of a rear axle of the tractor) is placed on a planned track line, the position coordinates of the tractor in the planned track are determined, and meanwhile, the head orientation of the tractor is determined according to the tangential direction of the planned reference point P in the planned track line. The mathematical expression is:
，
wherein,respectively two-dimensional plane coordinates and directions of the central point of the rear axle of the tractor,respectively two-dimensional plane coordinates and curve tangential directions of the planning reference point P in the planning track line.
In step 2, the curvature information of the obtained planned reference point in the planned trajectory line and the spatial change rate information of the planned reference point generally have different obtaining methods in different planned trajectory expression modes. The planned trajectory line of polynomial fitting can be directly solved according to a theoretical formula of curvature and change rate thereof, and the planned trajectory line described by a space discrete point mode usually only comprises the curvature at discrete points and the space change rate information of each order thereof, and at the moment, the curvature at a planned reference point P and the first-order and second-order space change rate thereof need to be solved by a space nearest point index and a space interpolation algorithm.
It should be noted that, no matter what expression mode is adopted for the planned trajectory, the curvature of the planned reference point in the planned trajectory and the first-order and second-order change rate information thereof are expressed as:
，
wherein P is a planning reference point of the tractor,for differential operation, lowercase +.>For the distance travelled by the vehicle>For planning the curvature of the reference point P in the planned trajectory, +.>，/>First and second rates of change of curvature with respect to distance travelled, respectively +.>Respectively planning reference points P to +.>，/>，/>Is a mapping relation of (a) to (b).
In step 3, the steering angle of the front wheel of the tractor and the time change rate thereof are estimated, and the specific principle is as follows: the curvature at the planned reference point determines the geometric steering radius, and the geometric steering radius R corresponds to that in fig. 2Wherein O is the rear of the tractorGeometric steering center point on the extension line of the wheel center vertical line, P is a planning reference point; and combining the assumption OF 'minimizing equivalent axis tire kinematic interference', namely positioning a geometric steering center O point on an extension line OF a perpendicular bisector OF a rear wheel OF the tractor, and then connecting the O point with a center F point OF the front wheel OF the tractor to enable a longitudinal center plane OF the front wheel OF the tractor to be perpendicular to a connecting line OF, so that the steering angle OF the front wheel OF the tractor can be obtained, and combining real-time motion information OF a vehicle and front and rear wheel bases OF the tractor, and solving the first-order and second-order time change rate OF the steering angle OF the front wheel OF the tractor, wherein the expression is as follows:
，
wherein,for geometric turning radius>For the front-rear wheelbase of the tractor, +.>，/>For time (I)>For differential operation, lowercase +.>For the distance travelled by the vehicle>For the speed of the tractor>I.e. +.>For the rate of change of the distance travelled by the tractor with respect to time, < >>Tangential acceleration for tractor travel, +.>I.e. +.>For the rate of change of the speed of the tractor with respect to time, < >>The first and second order change rates of the curvature of the planning reference point P with respect to time are respectively +.>For the steering angle of the front wheels of the tractor +.>，/>The first and second order rates of change of the steering angle of the front wheels of the tractor with respect to time, respectively, arctan represents the arctan function.
In step 4, according to the geometric steering center point O and the steering angle information of the front wheels of the tractor determined in step 3, the hinging angle and the information of the derivative of each step are solved by combining the assumption of minimizing the equivalent axis tire operational interference. The specific principle is as follows: and fixing the tractor according to the position and orientation information of the tractor, rotating and adjusting the hinge angle until the connecting line OQ of the geometric steering central point O and the center point Q of the equivalent axle of the trailer is vertical to the longitudinal central axis AQ of the trailer, wherein the hinge angle at the moment is the hinge angle under the assumption that the equivalent axle tire kinematic interference is minimized. From the geometric relationship, the solution of the articulation angle is expressed as:
，
wherein,for the hinge point A to the gaugeThe distance of the dividing reference point P, i.e. the distance of the hinge point A from the centre of the rear axle of the tractor,/->For the distance of the hinge point A to the center point Q of the equivalent axle of the trailer, i.e. the equivalent trailer wheelbase, then +.>Is the hinge angle. The trailer rotates counterclockwise around the hinge point to be positive and rotates clockwise to be negative.
Geometric equation for the hinge angle described aboveThe two sides are simultaneously derived, and the first-order derivative and the second-order derivative of the hinge angle under the assumption of 'minimizing equivalent axis tire kinematic interference' can be deduced, and are expressed as:
，
wherein,for the first derivative of the articulation angle, +.>Is the second derivative of the articulation angle.
In addition, in this embodiment, if the above-mentioned hinge angle and the first-order and second-order change rates are estimated, the following constraint conditions need to be satisfied:
，
in general terms, the process is carried out,if true, the constraint is expressed as:
，
when the steering angle of the front wheels of the tractor exceeds the upper limit of the constraint condition, the articulation angle is considered to reach a physical limit value and enter a locking state, and in the locking state, the articulation angle and the first-order and second-order change rate estimation thereof are expressed as:
，
wherein,estimated value for hinge angle in the locked state, < +.>And->The first-order and second-order change rates of the hinge angle in the locked state are respectively; sign represents a sign taking function;
and when the steering angle of the front wheels of the tractor returns to the upper limit of the constraint condition, the locking state is released.
It should be noted that, in this embodiment, the estimated articulation angle in step 4 has higher accuracy when the curvature of the planned trajectory line is constant and the vehicle enters into stable steering, and when the curvature of the planned trajectory line changes, the actual response of the articulation angle cannot converge to the geometric theoretical value in time due to the influence of the large inertia hysteresis of the vehicle in the dynamic steering process, so in this embodiment, a space distance constant is introduced, and in combination with the longitudinal speed of the tractor operation, in step 5, the geometric theoretical value of the articulation angle and its derivatives obtained in step 4 is subjected to first-order hysteresis processing, which is expressed as:
，
wherein,is a space distance constant, ">Conversion of the spatial distance constant into a first-order system time constant in the time domain, +.>For Laplase operator, +.>For the hinge angle after the first order dynamic hysteresis process,/->，/>The first-order and second-order change rates of the hinge angle with respect to time after the first-order dynamic hysteresis processing are respectively carried out.
In this embodiment, through the above, it is disclosed that the calculation of the articulated vehicle articulation angle based on the assumption of "minimizing equivalent axis tire kinematic interference" and the estimation results of the respective step change rates thereof in the planned trajectory line are all completed. In the development process of the automatic driving planning algorithm, the estimation result can be used for predicting the complete gesture of the hinged double-vehicle body in the planned track line, so as to predict the collision situation of the hinged vehicle and other dynamic traffic participants (such as pedestrians, passenger vehicles and the like) or static traffic elements (such as lane lines, stone piers and the like). In the autopilot simulation test, the estimation can be used to make a more reasonable perfect control deduction of the position and attitude of the articulated double body in the planned trajectory.
In order to illustrate the effect of the hinge angle estimation method of the above embodiment, referring to fig. 3 to 9, in another embodiment, a commercial hinge container truck operated in an unmanned demonstration operation area in Shanghai city is selected as a simulation experiment object. The characteristic geometrical parameters of the articulated vehicle according to the invention are shown in fig. 3.
In the embodiment, two planned space trajectories described by a four-order Bezier curve are selected as experimental working conditions, namely a high-speed lane change working condition and a low-speed U-shaped turning working condition, wherein the high-speed lane change speed is 60km/h, the low-speed U-shaped bending speed is 20km/h, the planned space trajectories of the high-speed lane change working condition are shown in fig. 4 (a), and the planned space trajectories of the low-speed U-shaped turning working condition are shown in fig. 4 (b). The fourth order Bessel parameter polynomial fitting coefficients corresponding to the two working conditions are shown in figure 5. Points A, B, C, D and E are characteristic points of a four-order Bezier curve, P and Q are extension points in the tangential direction of the Bezier curve end points, and the purpose of increasing the extension points is to enable the vehicle to start from steady-state linear motion, finish from steady-state linear motion after undergoing steering motion. The fourth-order Bezier curve parameter polynomial equation is as follows:
，
wherein,to take the value in the range of 0,1]Parameters of the interior->Respectively the abscissa and ordinate of the point on the curve,fitting coefficients of the abscissa and ordinate polynomial parameter equations are respectively obtained.
For the high-speed lane change condition shown in fig. 4 (a), the estimation result of the estimation method of the invention on the articulation angle of the articulated vehicle and the change rate of each step is shown in fig. 6, fig. 6 (a) is an estimated value of the articulation angle, fig. 6 (b) is an estimated value of the articulation angle rate, and fig. 6 (c) is an estimated value of the articulation angle acceleration. That is, FIG. 6 shows that the estimation method of the present invention gives reasonable estimation values for both the hinge angle and the first-order and second-order change rate thereof; the dynamically modified estimation results are more continuous and smoother relative to the geometric theory values. Fig. 7 shows the results of the hinge angle estimation by the estimation method of the present invention and the conventional method, and compares the results with the simulation experiment results of the professional vehicle dynamics simulation software TruckSim. The experimental result shown in fig. 7 shows that the absolute value of the hinge angle estimated by the traditional method is smaller, the estimated value of the hinge angle in the high-speed channel switching working condition is closer to the reference value of the simulation experiment, and the accuracy of estimation is remarkably improved.
For the low-speed U-bend condition shown in fig. 4 (b), the estimation results of the hinge angle and the change rate of each step thereof by adopting the estimation method of the present invention are shown in fig. 8, where fig. 8 (a) is a hinge angle estimation value, fig. 8 (b) is a hinge angle rate estimation value, and fig. 8 (c) is a hinge angle acceleration estimation value. Similar to Gao Suhuan working conditions, fig. 8 shows that the estimation method of the invention gives reasonable estimation to the hinge angle and the first-order and second-order change rates thereof, and the estimation result after dynamic correction is more continuous and smoother than the geometric theoretical value. Fig. 9 shows the results of the hinge angle estimation by the estimation method of the present invention and the conventional method, and compares the results with the simulation experiment results of the professional vehicle dynamics simulation software TruckSim. Similar to Gao Suhuan working conditions, fig. 9 shows that the absolute value of the hinge angle estimated by the conventional method is smaller, the estimated value of the hinge angle in the U-shaped curve working condition is closer to the reference value of the simulation experiment, and the accuracy of estimation is also obviously improved.
Based on the demonstration of the embodiment, the hinge angle estimation method of the articulated vehicle can reasonably estimate the hinge angle of the articulated vehicle and the first-order and second-order change rate thereof in a large-range working condition of high/low speed and large/small curvature thanks to the reasonable assumption of minimizing the equivalent axis tire kinematic interference, and the estimation precision is obviously improved compared with the traditional method.
In another embodiment of the present invention, a vehicle is disclosed that includes a tractor and a trailer and performs the articulation angle estimation method described above.
Through the embodiment, the technical scheme of the invention is disclosed, and compared with the prior art, the technical scheme of the invention has the following advantages:
1. the estimation method is based on the assumption of ' minimizing equivalent axis tire kinematic interference ', which is different from the assumption that the traction vehicle planning reference point and the trailer equivalent axis center point of the existing method are simultaneously positioned on the planning track ', and more accords with the dynamics and kinematic characteristics of the articulated vehicle when the articulated vehicle turns to drive, and the estimation result is more close to the actual value;
2. the estimation method of the invention uses the curvature at the specific track planning point and the space change rate thereof as a data basis, so that the estimated value of the hinge angle has a unique solution, and the reasonable choice and rejection problem of a plurality of solutions possibly existing in the large-curvature alternating planning track by the existing method are avoided;
3. according to the estimation method, a space distance constant is introduced, and the tractor speed is combined, so that a geometric theoretical value of a time domain first-order hysteresis system acting on an estimated result of the hinge angle is constructed, the dynamic change characteristic of the estimated result in a planned track is more practical, and the requirement of estimated rationality on the curvature continuity of the planned track is reduced;
4. the method estimates the first-order and second-order change rate of the hinge angle, namely the hinge angular velocity and the angular acceleration, and simultaneously estimates the hinge angle, so that the estimation result can support the prediction of high-order information such as the velocity, the acceleration, the angular velocity, the angular acceleration and the like of the hinge double-car body.
The above embodiments are only for illustrating the technical solution of the present invention, but not for limiting, and other modifications and equivalents thereof by those skilled in the art should be included in the scope of the claims of the present invention without departing from the spirit and scope of the technical solution of the present invention.
Claims (9)
1. A method for estimating a articulation angle of a commercial articulated vehicle in a planned trajectory, comprising the steps of:
step 1: taking a planning track line and a planning reference point as inputs, placing the planning reference point on the planning track line, and determining the position coordinates and the orientation of the tractor through the coordinates and the tangential direction of the planning reference point;
step 2: acquiring curvature information of a planning reference point in a planning track line and spatial change rate information of the planning reference point;
step 3: determining a geometric steering radius according to curvature information in a planned track, positioning a geometric steering center point on an extension line of a perpendicular bisector of a rear wheel of the tractor, and estimating a steering angle and a time change rate of the front wheel of the tractor by combining the driving speed, acceleration information and a front-rear wheelbase of the tractor to make a connecting line from the center of the front wheel of the tractor to the geometric steering center be perpendicular to a longitudinal center plane of the front wheel of the tractor based on the curvature and the space change rate obtained in the step 2;
step 4: based on the assumption of the minimized equivalent axle tire kinematic interference, connecting lines from the front wheel center point of the tractor, the rear axle center point of the tractor and the trailer equivalent axle center point to the geometric steering center are perpendicular to the longitudinal center plane of each wheel, and the steering angle of the front wheel of the tractor and the time change rate thereof obtained in the step 3 are combined to solve the hinging angle and the information of each derivative thereof;
wherein, fixed tractor according to the position and the orientation information of tractor, adjustment hinge angle, until geometric steering central point O and trailer equivalent axle central point Q's line OQ and trailer longitudinal center axis AQ are perpendicular, then the hinge angle expresses as:
，
wherein,for the distance of the hinge point a from the planned reference point P, i.e. the distance of the hinge point a from the center of the rear axle of the tractor, +.>For the distance of the hinge point A to the center point Q of the equivalent axle of the trailer, i.e. the equivalent trailer wheelbase,/->For the front-rear wheelbase of the tractor, +.>，/>For the steering angle of the front wheels of the tractor +.>Is a hinge angle;
step 5: and (3) according to the longitudinal speed of the tractor, performing hysteresis processing on the articulation angle and each derivative thereof obtained in the step (4).
2. The method for estimating a articulation angle of a commercial articulated vehicle in a planned trajectory according to claim 1, wherein: in step 1, the planning reference point is selected as the center point of the rear axle of the tractor, and the planning reference point coordinates and tangential expression are:
，
wherein,respectively two-dimensional plane coordinates and directions of the central point of the rear axle of the tractor,respectively two-dimensional plane coordinates and curve tangential directions of the planning reference point P in the planning track line.
3. The method for estimating an articulation angle of a commercial articulated vehicle in a planned trajectory according to claim 2, wherein: in step 2, the spatial change rate information at the planning reference point includes first-order and second-order change rate information of the driving distance of the tractor, and then the curvature of the planning reference point in the planning track line and the first-order and second-order change rate information thereof are expressed as follows:
，
wherein,representing the curvature of the tractor planning reference point P in the planning track and its first and second rate of change with respect to the distance travelled, respectively,/->Representing the mapping relationship from the planning reference point P to the curvature and the first and second order, respectively.
4. A method of estimating a articulation angle of a commercial articulated vehicle in a planned trajectory according to claim 3, characterized by: in step 3, the geometric steering radius R of the tractor is obtained by planning the curvature of the reference point, expressed as:
，
wherein O is a geometric steering center point on an extension line of a perpendicular bisector of a rear wheel of the tractor, and P is a planning reference point;
connecting the point O with the center point F OF the front wheel OF the tractor to enable the longitudinal center plane OF the front wheel OF the tractor to be perpendicular to the connecting line OF, obtaining the steering angle OF the front wheel OF the tractor, and solving the first-order and second-order time change rate OF the steering angle OF the front wheel OF the tractor by combining the real-time motion information OF the vehicle and the front-rear wheelbase OF the tractor, wherein the expression is as follows:
，
wherein,for the front-rear wheelbase of the tractor, +.>For the speed of the tractor>Tangential acceleration for tractor travel, +.>The first and second order change rates of the curvature of the planning reference point P with respect to time are respectively +.>The steering angle of the front wheel of the tractor and the first-order and second-order change rates of the steering angle and the time of the front wheel of the tractor are respectively.
5. The method for estimating an articulation angle of a commercial articulated vehicle in a planned trajectory of claim 4, wherein: geometric equation for articulation angleAnd simultaneously deriving the two sides, and deriving first and second derivatives of the hinge angle, wherein the first and second derivatives are expressed as:
，
wherein,for the first derivative of the articulation angle, +.>Is the second derivative of the articulation angle.
6. The method for estimating an articulation angle of a commercial articulated vehicle in a planned trajectory according to claim 5, wherein: the constraint conditions to be satisfied are:
，
if it isThe constraint is expressed as:
，
the state of the hinge angle is determined by the constraint conditions described above.
7. The method for estimating an articulation angle of a commercial articulated vehicle in a planned trajectory of claim 6, wherein: if the steering angle of the front wheels of the tractor exceeds the upper limit of the constraint condition, the articulation angle reaches a physical limit value and enters a locking state, and under the locking state, the articulation angle and the first-order and second-order change rate estimation thereof are expressed as follows:
，
wherein,estimated for the articulation angle in the locked state, +.>And->The first-order and second-order change rates of the hinge angle in the locked state are respectively;
and if the steering angle of the front wheels of the tractor returns to the upper limit of the constraint condition, the locking state is released.
8. The method for estimating a articulation angle of a commercial articulated vehicle in a planned trajectory according to claim 1, wherein: in step 5, in the dynamic steering process of the vehicle, a first-order hysteresis process is performed on the articulation angle and each derivative thereof, which is expressed as:
，
wherein,is a space distance constant, ">Conversion of the spatial distance constant into a first-order system time constant in the time domain, +.>For Laplase operator, +.>The hinge angle after the first-order dynamic hysteresis treatment and the first-order and second-order change rates thereof are respectively adopted.
9. A vehicle, characterized in that: the vehicle comprising a tractor and a trailer and performing the articulation angle estimation method of any of the preceding claims 1-8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311549019.7A CN117272693B (en) | 2023-11-21 | 2023-11-21 | Method for estimating articulation angle of commercial articulated vehicle in planned track |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311549019.7A CN117272693B (en) | 2023-11-21 | 2023-11-21 | Method for estimating articulation angle of commercial articulated vehicle in planned track |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117272693A CN117272693A (en) | 2023-12-22 |
| CN117272693B true CN117272693B (en) | 2024-03-01 |
Family
ID=89202941
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311549019.7A Active CN117272693B (en) | 2023-11-21 | 2023-11-21 | Method for estimating articulation angle of commercial articulated vehicle in planned track |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117272693B (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112004735A (en) * | 2018-04-16 | 2020-11-27 | 沃尔沃卡车集团 | Method for estimating a maximum safe articulation angle to be used in reversing a vehicle combination |
| CN115503758A (en) * | 2022-10-23 | 2022-12-23 | 北京清维智驾科技有限公司 | Method for planning motion trail of automatic driving articulated vehicle |
| CN115717898A (en) * | 2022-11-07 | 2023-02-28 | 上海友道智途科技有限公司 | Automatic driving planning trajectory perfect deduction method for commercial articulated vehicle |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10974121B2 (en) * | 2015-07-16 | 2021-04-13 | Blast Motion Inc. | Swing quality measurement system |
| US20200001920A1 (en) * | 2018-07-02 | 2020-01-02 | Ohio State Innovation Foundation | Systems and methods for preventing a jackknife condition in a tractor-trailer system |
-
2023
- 2023-11-21 CN CN202311549019.7A patent/CN117272693B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112004735A (en) * | 2018-04-16 | 2020-11-27 | 沃尔沃卡车集团 | Method for estimating a maximum safe articulation angle to be used in reversing a vehicle combination |
| CN115503758A (en) * | 2022-10-23 | 2022-12-23 | 北京清维智驾科技有限公司 | Method for planning motion trail of automatic driving articulated vehicle |
| CN115717898A (en) * | 2022-11-07 | 2023-02-28 | 上海友道智途科技有限公司 | Automatic driving planning trajectory perfect deduction method for commercial articulated vehicle |
Non-Patent Citations (2)
| Title |
|---|
| Research on a Path-Tracking Control System for Articulated Tracked Vehicles;Da Cui 等;《Journal of Mechanical Engineering》;第311-324页 * |
| 基于模糊快速滑模的农用牵引车辆直线路径跟踪控制;黄文岳 等;《农业机械学报》;第1-10页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117272693A (en) | 2023-12-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111845774B (en) | Automatic driving automobile dynamic trajectory planning and tracking method based on transverse and longitudinal coordination | |
| Pérez et al. | Cascade architecture for lateral control in autonomous vehicles | |
| Liang et al. | Automatic parking path optimization based on bezier curve fitting | |
| CN107901917B (en) | A kind of automatic driving vehicle Trajectory Tracking Control method based on sliding coupling estimation of trackslipping | |
| CN104977933A (en) | Regional path tracking control method for autonomous land vehicle | |
| CN110696819B (en) | Automatic parking path planning method and system | |
| CN114435470B (en) | Automatic reversing control method and device, vehicle and storage medium | |
| CN111930126B (en) | Navigation deviation rectifying method based on differential wheel set AGV | |
| CN112578672B (en) | Unmanned vehicle trajectory control system based on chassis nonlinearity and trajectory control method thereof | |
| CN111158379B (en) | Steering wheel zero-bias self-learning unmanned vehicle track tracking method | |
| CN113433827B (en) | Centroid change sand blasting and rust removing parallel robot moving platform track tracking control method | |
| CN109292018B (en) | Four-wheel steering track tracking control method based on coaxial wheel leg structure | |
| CN111679676A (en) | AGV movement track control method | |
| CN117272693B (en) | Method for estimating articulation angle of commercial articulated vehicle in planned track | |
| CN110362088B (en) | Tracking control system and method suitable for unmanned straddle carrier | |
| Zhang et al. | Learning end-to-end inertial-wheel odometry for vehicle ego-motion estimation | |
| CN114044003B (en) | Tracking control method for front-rear double-axle steering vehicle | |
| CN113914406B (en) | Electric loader path tracking system and control method | |
| CN114291117A (en) | Four-wheel steering unmanned vehicle trajectory tracking control method | |
| Mohamed-Ahmed et al. | Non-linear control based on state estimation for the convoy of autonomous vehicles | |
| CN114489038B (en) | Navigation control method of agriculture and forestry vehicle based on reinforcement learning | |
| CN117519190B (en) | Novel articulated vehicle control method | |
| CN117227744A (en) | Autonomous vehicle path tracking control method based on high-speed nonsingular switching sliding mode | |
| KR20110121129A (en) | Calibrating method of odometry error of 4-wheel car | |
| Mahmoud et al. | Non-linear Control based on State Estimation for the Convoy of Autonomous Vehicles |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |