[go: up one dir, main page]

CN116414120A - Path tracking method, path tracking device, electronic equipment and storage medium - Google Patents

Path tracking method, path tracking device, electronic equipment and storage medium Download PDF

Info

Publication number
CN116414120A
CN116414120A CN202211677036.4A CN202211677036A CN116414120A CN 116414120 A CN116414120 A CN 116414120A CN 202211677036 A CN202211677036 A CN 202211677036A CN 116414120 A CN116414120 A CN 116414120A
Authority
CN
China
Prior art keywords
vehicle
point
feedforward
motion model
rear axle
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
Application number
CN202211677036.4A
Other languages
Chinese (zh)
Inventor
刘凯
曹世卓
周小成
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Uisee Technologies Beijing Co Ltd
Original Assignee
Uisee Technologies Beijing Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Uisee Technologies Beijing Co Ltd filed Critical Uisee Technologies Beijing Co Ltd
Priority to CN202211677036.4A priority Critical patent/CN116414120A/en
Publication of CN116414120A publication Critical patent/CN116414120A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0253Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/028Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using a RF signal

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)

Abstract

The embodiment of the disclosure discloses a path tracking method, a device, electronic equipment and a storage medium, wherein the method comprises the following steps: according to the current speed of the vehicle and the current front wheel deflection angle, determining the estimated pose of the vehicle after a preset moment based on a motion model of the vehicle, wherein the estimated pose comprises the estimated position coordinates of the rear axle center of the motion model; according to the estimated pose and the estimated position coordinates of the rear axle center of the motion model, a projection point of the rear axle center of the motion model on a desired path, and a feedforward curvature radius and a feedforward heading at the projection point are obtained; generating a reference circle according to the feedforward curvature radius and the feedforward heading; determining the position coordinates of a pre-aiming point on a reference circle according to the pre-estimated pose of the vehicle after the preset moment and the preset pre-aiming distance; and determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle. The influence of the path curvature feedforward and the vehicle steering delay is comprehensively considered, and high-precision path tracking can be realized.

Description

Path tracking method, path tracking device, electronic equipment and storage medium
Technical Field
The disclosure relates to the technical field of automatic driving, and in particular relates to a path tracking method, a path tracking device, electronic equipment and a storage medium.
Background
The application fields of the automatic driving technology are becoming wider and wider, including, for example, container intelligent transfer vehicles applied in ports, express delivery vehicles applied in the express delivery field, unmanned taxi vehicles applied in the taxi field, and the like.
In the prior art, for vehicles in all application fields, a mature pure tracking (PP) algorithm is generally adopted, the pure tracking algorithm considers a geometric model of the vehicle, and can achieve a good tracking effect and show good stability under the conditions of discontinuous expected path curvature and large transverse deviation. The inventors have found in the course of implementing the invention that: the pure tracking algorithm is easy to generate larger tracking deviation when tracking a curve with larger curvature.
Disclosure of Invention
To solve or at least partially solve the above technical problems, embodiments of the present disclosure provide a path tracking method, apparatus, electronic device, and storage medium.
In a first aspect, the present disclosure provides a path tracking method, including:
acquiring the current speed and the current front wheel deflection angle of the vehicle;
determining an estimated pose of the vehicle after a preset moment based on a motion model of the vehicle according to the current speed and the current front wheel deflection angle of the vehicle, wherein the estimated pose comprises an estimated position coordinate of a rear axle center of the motion model;
According to the estimated pose and the estimated position coordinates of the rear axle center of the motion model, a projection point of the rear axle center of the motion model on a desired path, and the feedforward curvature radius and the feedforward heading at the projection point are obtained;
generating a reference circle according to the feedforward curvature radius and the feedforward heading, wherein the reference circle is tangential to the expected path at the projection point;
determining the position coordinates of a pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle at the preset moment and the preset pre-aiming distance;
and determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
In a second aspect, the present disclosure further provides a path tracking apparatus, including:
the first acquisition module is used for acquiring the current speed and the current front wheel deflection angle of the vehicle;
the pose estimating module is used for determining an estimated pose of the vehicle after a preset moment based on a motion model of the vehicle according to the current speed and the current front wheel deflection angle of the vehicle, wherein the estimated pose comprises an estimated position coordinate of a rear axle center of the motion model;
the second acquisition module is used for acquiring a projection point of the rear axle center of the motion model on a desired path, and the feedforward curvature radius and the feedforward heading at the projection point according to the estimated pose and the estimated position coordinate of the rear axle center of the motion model;
The reference circle generation module is used for generating a reference circle according to the feedforward curvature radius and the feedforward heading, and the reference circle is tangential to the expected path at the projection point;
the pre-aiming point determining module is used for determining the position coordinates of the pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle after the preset moment and the preset pre-aiming distance;
and the control quantity determining module is used for determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
In a third aspect, the present disclosure also provides an electronic device, including:
one or more processors;
a storage means for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the path tracking method as described above.
In a fourth aspect, the present disclosure also provides a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements a path tracking method as described above.
Compared with the prior art, the technical scheme provided by the embodiment of the disclosure has the following advantages:
According to the technical scheme, the projection point of the rear axle center of the motion model on the expected path, and the feedforward curvature radius and the feedforward heading at the projection point are obtained through setting the estimated position coordinates according to the estimated pose and the rear axle center of the motion model; generating a reference circle according to the feedforward curvature radius and the feedforward heading, wherein the reference circle is tangential to the expected path at the projection point; determining the position coordinates of a pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle at the preset moment and the preset pre-aiming distance; and determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle, wherein the influence of path curvature feedforward and vehicle steering delay is comprehensively considered, and high-precision path tracking can be realized.
Drawings
The above and other features, advantages, and aspects of embodiments of the present disclosure will become more apparent by reference to the following detailed description when taken in conjunction with the accompanying drawings. The same or similar reference numbers will be used throughout the drawings to refer to the same or like elements. It should be understood that the figures are schematic and that elements and components are not necessarily drawn to scale.
Fig. 1 is a flowchart of a path tracking method according to an embodiment of the present disclosure;
FIG. 2 is a schematic diagram of a path tracking method according to an embodiment of the present disclosure;
FIG. 3 is a schematic diagram of a principle of calculating a predicted pose provided by an embodiment of the present disclosure;
FIG. 4 is a schematic diagram of finding proxels provided by embodiments of the present disclosure;
FIG. 5 is a schematic view of a reference circle provided in an embodiment of the present disclosure;
FIG. 6 is a schematic diagram of finding an aiming point based on a predicted pose and a pre-aiming distance provided by an embodiment of the present disclosure;
FIG. 7 is a schematic diagram of path tracking after selecting a control point according to an embodiment of the present disclosure;
FIG. 8 is a schematic illustration of a steering motion of a vehicle provided by an embodiment of the present disclosure;
FIG. 9 is a schematic illustration of steering motion of a vehicle in a dual-axle mode according to an embodiment of the present disclosure;
FIG. 10 is a schematic diagram of a path tracking device according to an embodiment of the disclosure;
fig. 11 is a schematic structural diagram of an electronic device in an embodiment of the disclosure.
Detailed Description
Embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. While certain embodiments of the present disclosure have been shown in the accompanying drawings, it is to be understood that the present disclosure may be embodied in various forms and should not be construed as limited to the embodiments set forth herein, but are provided to provide a more thorough and complete understanding of the present disclosure. It should be understood that the drawings and embodiments of the present disclosure are for illustration purposes only and are not intended to limit the scope of the present disclosure.
Fig. 1 is a flowchart of a path tracking method according to an embodiment of the present disclosure, which is applicable to an automatic driving situation, and the method may be performed by a vehicle. Fig. 2 is a schematic diagram of a path tracking method according to an embodiment of the present disclosure. As shown in fig. 1 and 2, the method may specifically include:
s110, acquiring the current speed and the current front wheel deflection angle of the vehicle.
The current speed and the current front wheel deflection angle in the step can be acquired in real time by corresponding equipment or sensors.
S120, according to the current speed and the current front wheel deflection angle of the vehicle, determining the estimated pose of the vehicle after the preset moment based on the motion model of the vehicle, wherein the estimated pose comprises the estimated position coordinates of the rear axle center of the motion model.
The motion model of the vehicle is a motion model that depicts the steering characteristics of the vehicle. In practice, the corresponding setting may be made according to the steering mode of the vehicle.
In one embodiment, a container intelligent transfer vehicle for use in a port generally includes a front axle steering mode, a rear axle steering mode, a crab steering mode, and a double axle steering mode. And respectively setting a motion model of the vehicle corresponding to the front axle steering mode, a motion model of the vehicle corresponding to the rear axle steering mode, a motion model of the vehicle corresponding to the crab steering mode and a motion model of the vehicle corresponding to the double-axle steering mode. The motion model of the vehicle used in performing this step corresponds to the steering mode actually used by the vehicle. This arrangement enables accurate path tracking.
Optionally, the motion model of the vehicle is an ackerman steering model.
In one embodiment, the implementation method of S120 includes: calculating the instantaneous turning radius of the vehicle and the circle center coordinates of the turning position according to the current front wheel deflection angle and the current position coordinates of the rear axle center of the motion model; calculating a vehicle course angle of the vehicle after a preset moment according to the instantaneous turning radius and the current speed; based on the motion model, calculating the estimated position coordinate of the rear axle center of the motion model at the preset moment according to the center coordinate of the circle at the turning position and the course angle of the vehicle at the preset moment.
Illustratively, the front wheel angle delta is based on the current speed of the vehicle f And predicting the pose after the time t. Wherein t is steering delay, which can be obtained through calibration. Fig. 3 is a schematic diagram of a principle of calculating a predicted pose according to an embodiment of the present disclosure. As shown in fig. 3, the instantaneous turning radius R of the present vehicle is calculated from the front wheel slip angle thereof 1 And the coordinates of the center of the curve (x 1 ,y 1 ):
Figure SMS_1
x 1 =x 0 -R 1 sinΨ
y 1 =y 0 +R 1 cosΨ
Wherein, (x) 0 ,y 0 ) Is the current position coordinate of the center of the rear axle of the motion model, ψ is the course angle of the vehicle, l fr Is the wheelbase of the vehicle.
According to the current speed V of the vehicle, calculating the heading angle of the vehicle after the moment t:
Ψ t =Ψ+Vt/R 1
Then the updated heading angle ψ t Setting to [0,2 pi ]]。
Finally, the horizontal and vertical coordinates of the predicted pose are calculated:
x t =x 1 +R 1 sinΨ t
y t =y 1 -R 1 cosΨ t
s130, according to the estimated pose and the estimated position coordinates of the rear axle center of the motion model, a projection point of the rear axle center of the motion model on a desired path, and a feedforward curvature radius and a feedforward heading at the projection point are obtained.
The specific implementation method of S130 is various, which is not limited in this application. In one embodiment, optionally, S130 includes: calculating the distance from each discrete road point on the expected path to the position coordinate of the rear axle center of the motion model at the preset moment, and acquiring a road section between the ith discrete road point and the (i+1) th discrete road point closest to the distance as a closest road section; acquiring the distance from a projection point of the rear axle center of the motion model on the nearest road section to an ith discrete road point, the feedforward curvature radius at the ith discrete road point and the feedforward heading at the ith discrete road point, and the distance from the projection point of the rear axle center of the motion model on the nearest road section to an (i+1) th discrete road point, the feedforward curvature radius at the (i+1) th discrete road point and the feedforward heading at the (i+1) th discrete road point; acquiring a feedforward curvature radius at a projection point according to the distance from the projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point, the feedforward curvature radius at the ith discrete road point, the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point and the feedforward curvature radius at the (i+1) th discrete road point; and acquiring the feedforward course at the projection point according to the distance from the projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point and the feedforward course at the ith discrete road point, the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point and the feedforward course at the (i+1) th discrete road point.
Illustratively, the road segment closest to the predicted pose on the desired path is found first, and then the abscissa (x) at the projection point P is calculated p ,y p ) And the feedforward curvature radius and the feedforward heading at the projection point P are obtained through weighted average.
Fig. 4 is a schematic diagram of finding a proxel according to an embodiment of the present disclosure. Specifically, the distance from each discrete road point on the expected path to the center of the rear axle of the ackerman steering model is calculated, and the road section with the nearest predicted pose is obtained. Referring to fig. 4, assume that the nearest discrete waypoint index is i, and that the coordinates of the discrete waypoint i are (x i ,y i ) The coordinates of the discrete waypoint i+1 are (x i+1 ,y i+1 ) The coordinates at the projection point are:
Figure SMS_2
wherein k= (y) i+1 -y i )/(x i+1 -x i ) For the slope of discrete waypoint i to discrete waypoint i+1, b=y i -kx i ,(x t ,y t ) The transverse and longitudinal coordinates of the pose are predicted.
Let d i And d i+1 The distance from the projection point P to the i-th point and the i+1th point is represented. R is R i And R is i+1 Representing the feedforward curvature radius at the i point and the i+1 point, the feedforward curvature radius at the projection point is obtained by weighted summation of the feedforward curvature radius at the i point and the i+1 point:
R ref =d i *R i+1 +d i+1 *R i (9)
and according to the feedforward heading weighted summation of the ith point and the (i+1) th point, obtaining the feedforward heading at the projection point:
Figure SMS_3
wherein θ i And theta i+1 Is the feed forward heading at the i-th point and i+1-th point.
And S140, generating a reference circle according to the feedforward curvature radius and the feedforward heading, wherein the reference circle is tangential to the expected path at the projection point.
The specific implementation method of S140 is various, which is not limited in this application. In one embodiment, optionally, S140 includes: and acquiring the center coordinates of a reference circle according to the position coordinates of the projection point of the rear axle center of the motion model on the nearest road section, the feedforward curvature radius and the feedforward heading, and generating the reference circle by taking the feedforward curvature radius as the radius.
Illustratively, the radius of curvature R of the feedforward at point P on the desired path ref And (3) making a circle, making the circle tangent to the expected path at the point P, and marking the circle as a reference circle. Fig. 5 is a schematic diagram of a reference circle according to an embodiment of the disclosure. As shown in fig. 5. Centre of reference circle (x) ref ,y ref ) The coordinates of (2) are:
x ref =x p +R ref sinθ p
y ref =y p -R ref cosθ p (11)
s150, determining the position coordinates of the pre-aiming point on a reference circle according to the pre-estimated pose of the vehicle at the preset moment and the preset pre-aiming distance.
The specific implementation method of S150 is various, which is not limited in this application. In one embodiment, optionally, S150 includes: transforming the center coordinates of the reference circle to the local coordinate system of the estimated pose to obtain the center coordinates of the reference circle under the local coordinate system of the estimated pose; acquiring a preset pre-aiming distance; taking the position coordinate of the rear axle center of the motion model after the preset moment as a circle center and the pretightening distance as a radius to obtain a pretightening circle; and acquiring the position coordinates of the pre-aiming point based on the reference circle and the pre-aiming circle under the local coordinate system of the pre-estimated pose.
Optionally, a preset pretightening distance p is obtained d Comprising: p is p d =d const +Vt 0 The method comprises the steps of carrying out a first treatment on the surface of the Wherein d const Is a preset distance value, V is the current speed of the vehicle, t 0 Is a preset time value.
Optionally, acquiring the position coordinates of the pre-aiming point based on the reference circle and the pre-aiming circle under the local coordinate system of the pre-estimated pose includes: when no intersection point exists between the reference circle and the pretightening circle under the local coordinate system of the pre-estimated pose, determining the position coordinate of the pretightening point according to the pretightening distance; and when the reference circle under the local coordinate system of the predicted pose and the pretightening circle have at least one intersection point, determining the position coordinates of the pretightening point according to the pretightening distance, the feedforward curvature radius and the circle center coordinates of the reference circle under the local coordinate system of the predicted pose.
For convenience in subsequent calculation of the pre-aiming point, the reference circle coordinates are transformed under the local coordinate system of the vehicle prediction pose,
x cen =x ref -x t
y cen =y ref -y t
wherein, (x) cen ,y cen ) Predicting the coordinates of the center of a reference circle under a pose local coordinate system for a vehicle, (x) ref ,y ref ) Is the center of a reference circle (x) before coordinate transformation t ,y t ) The transverse and longitudinal coordinates of the pose are predicted.
Taking the center of the rear axle of the Ackerman steering model as the center of a circle and taking the pretightening distance p as the center d And (3) making a circle for the radius, and solving an intersection point of the circle and the reference circle to serve as a pre-aiming point. Fig. 6 is a schematic diagram of finding an aiming point based on a predicted pose and a pre-aiming distance, provided by an embodiment of the present disclosure. As shown in fig. 6. The pre-aiming distance calculating method comprises the following steps:
p d =d const +Vt O (13)
Wherein d const Is a preset distance value, t 0 The time value is preset, and the time value can be set according to actual conditions.
Let d c The table refers to the distance from the center of a circle to the center of a rear axle of the ackerman steering model:
Figure SMS_4
according to the pretightening distance p d And d c The relationship between the pre-aiming circle and the reference circle is as follows:
if p is d <|d c -|R ref Either I or p d >|d c +|R ref And if the preset circle is not intersected with the reference circle, the preset circle is not intersected with the reference circle.
If |d c -|R ref ||≤p d ≤|d c +|R ref And (3) if the preset circle and the reference circle have at least one intersection point.
When there is no intersection point between the two circles, a pre-aiming point can be designated. Alternatively, if R ref > 0, then the pretightening point can be designated as (-p) a 0.0), or if Rr ef If the pre-aiming point is less than or equal to 0, the pre-aiming point is (-p) a ,0.0)。
When at least one intersection point exists between the two circles, a pretightening point (x preview ,y preview ) The coordinate formula of (2) is:
Figure SMS_5
Figure SMS_6
the parameters in the above formula are respectively taken:
a=2p d x cen ,b=2p a y cen
Figure SMS_7
p=a 2 +b 2 ,q=2bK,/>
Figure SMS_8
M=2aK,/>
Figure SMS_9
Figure SMS_10
and (5) bringing the parameters into a coordinate formula to obtain the coordinates of the pre-aiming point.
S160, determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
The specific implementation method of S160 is various, which is not limited in this application. In one embodiment, optionally, S160 includes: acquiring current position coordinates of a control point of a vehicle; and determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the current position coordinates of the control point of the vehicle so that the control point of the vehicle reaches the pre-aiming point along a circular arc path.
In the present application, the objective of the path tracking is to make the control point of the ackerman steering model reach the pre-aiming point along an arc. When using the path tracking method provided by the present application, a control point of the vehicle needs to be predetermined. The specific selection position of the vehicle control point is not limited in the application. In some embodiments, the rear wheel center of the ackerman steering model may be selected as the control point. In other embodiments, a point between the front wheel center and the rear wheel center of the ackerman steering model may be selected as the control point.
For a container intelligent transfer vehicle, optionally, a midpoint between the front wheel center and the rear wheel center is selected as the control point. This is because the feasible roads of the port loading and unloading area and the horizontal transport area are generally narrow, and the intelligent container transfer vehicle has a long vehicle body and a wide vehicle body, which requires the intelligent container transfer vehicle to have high-precision path tracking capability. The midpoint between the front wheel center and the rear wheel center is set to be selected as a control point, so that a sufficient safety margin can be ensured in path tracking.
Fig. 7 is a schematic diagram of path tracking after selecting a control point according to an embodiment of the present disclosure. Referring to fig. 7, after the control point is selected, the goal of the path tracking is to pass the control point along an arc through the pre-aiming point. In FIG. 7, G is a pretightening point, A is a control point, R A And R is the turning radius at the control point A and the rear axle center N1, L respectively la For the pre-aiming distance of the center of the rear axle, L A For the distance from the pretightening point G to the control point A, the distance from the control point A to the center N1 of the rear axle is recorded as t d ,a A For connecting AG from control point A to pretightening point G with vehicleAnd the included angle of the vehicle wheelbase, O is the center of a concentric circle.
In one embodiment, determining the front wheel control amount of the vehicle from the position coordinates of the pre-aiming point and the current position coordinates of the control point of the vehicle includes:
the front wheel control amount delta of the vehicle is determined according to the following formula f
Figure SMS_11
Wherein a is the equivalent front wheel deflection angle of the rear axle center of the running model of the vehicle, a A Is the equivalent front wheel deflection angle of the control point of the vehicle, L A For the distance of the control point of the vehicle to the pre-aiming point, l fr T is the wheelbase of the vehicle d Is the distance from the control point of the vehicle to the center of the rear axle.
According to the technical scheme provided by the embodiment of the disclosure, the expected path is divided into a plurality of relatively independent small road sections, each divided small road section is regarded as an arc, and the pretightening point corresponding to each divided small road section is determined so as to control the control point of the vehicle to reach the pretightening point along the arc-shaped path, so that the reduction of complexity can be realized, and tracking deviation of the vehicle when the vehicle travels along a curve with a larger curvature is reduced. The path tracking method provided by the embodiment of the disclosure comprehensively considers the influence of the path curvature feedforward and the vehicle steering delay, and can realize high-precision path tracking.
In addition, no matter which position the vehicle control point is arranged according to the requirement, the path tracking method provided by the application can be used, so that the path tracking method provided by the embodiment of the disclosure can be flexibly adapted to the vehicle control points at different positions, and enough safety margin can be ensured when the vehicle turns.
Based on the above technical solution, in one embodiment, before S120, the method further includes: acquiring a vehicle steering working mode of a current road section; and determining a running model of the vehicle at the current road section according to the vehicle steering working mode of the current road section, wherein the running model of the vehicle is correspondingly arranged with the vehicle steering working mode, and the running model of the vehicle is obtained by simplifying a standard vehicle kinematic model according to characteristic parameters of the vehicle steering working mode. Therefore, the motion model of the vehicle can be corresponding to the steering mode actually used by the vehicle, and the tracking control precision of the steering of the vehicle is improved.
Further, acquiring a vehicle steering working mode of the current road section includes: determining the road curvature of the current road section; and when the curvature of the road is greater than or equal to a first threshold value, determining that the vehicle steering working mode of the current road section is a double-axle steering mode, otherwise, determining that the vehicle steering mode of the current road section is a front axle steering mode or a rear axle steering mode. In practice, since the two-axle steering mode can achieve a smaller turning radius, if the road curvature of the current road section is greater than or equal to the first threshold value, meaning that the current road section is passed through without collision, it is necessary to use a steering mode that can achieve a very small turning radius. The arrangement can ensure that the vehicle has higher steering precision when automatically steering, and the collision probability is sufficiently reduced.
Further, before determining the road curvature of the current road segment, the method may further include: determining whether a vehicle steering mode is appointed for a current road section in the road network file; if yes, determining the vehicle steering mode designated for the current road section in the road network file as the vehicle steering mode of the current road section. The road network file records a plurality of road section designated vehicle steering modes. The method can ensure that the vehicle motion model is accurate when the path tracking method provided by the application is executed finally, and further ensure that the front wheel control quantity of the finally obtained vehicle is accurate.
If the vehicle steering working mode of the current road section is a front axle steering mode, simplifying according to a vehicle standard kinematics model to obtain a vehicle motion model corresponding to the center of a rear axle of the vehicle, wherein the vehicle motion model is as follows:
Figure SMS_12
wherein, (x) B ,y B ) Is the position coordinates of the center of the rear axle of the vehicle,
Figure SMS_13
represents the rate of change on the X-axis, +.>
Figure SMS_14
Representing the rate of change on the Y-axis; delta f Is the equivalent front wheel deflection angle of the front axle, < >>
Figure SMS_15
Is the change rate of the heading angle of the vehicle; lfr the wheelbase of the vehicle; v is the vehicle speed, the rear axle center of the vehicle being the rear axle center of the motion model of the vehicle.
If the vehicle steering working mode of the current road section is a rear axle steering mode, simplifying according to a vehicle standard kinematics model to obtain a vehicle motion model corresponding to the center of the rear axle of the vehicle, wherein the vehicle motion model is as follows:
Figure SMS_16
Wherein, (x) A ,y A ) Is the position coordinates of the center of the front axle of the vehicle,
Figure SMS_17
represents the rate of change on the X-axis, +.>
Figure SMS_18
Representing the rate of change on the Y-axis; delta r As the equivalent front wheel deflection angle of the rear axle, ψ' =ψ+pi, ψ is the heading angle of the vehicle, +.>
Figure SMS_19
Representing the change rate of the heading angle; l (L) fr Is the wheelbase of the vehicle; v is the vehicle speed, the front axle center of the vehicle being the rear axle center of the vehicle motion model.
If the vehicle steering working mode of the current road section is a double-axle steering mode, simplifying according to a vehicle standard kinematics model to obtain a vehicle motion simplified model corresponding to the center of a rear axle of the vehicle, wherein the vehicle motion simplified model is as follows:
Figure SMS_20
wherein, (x) C ,y C ) Is the position coordinates of the center of the vehicle wheelbase of the vehicle,
Figure SMS_21
represents the rate of change on the X-axis, +.>
Figure SMS_22
Representing the rate of change on the Y-axis; delta f Is the equivalent front wheel deflection angle of the front axle, < >>
Figure SMS_23
Is the change rate of the heading angle of the vehicle; l (L) fr Is the wheelbase of the vehicle; v is the vehicle speed and the vehicle wheelbase center point of the vehicle is taken as the center of the rear axle of the vehicle motion model.
Fig. 8 is a schematic view of a steering motion of a vehicle according to an embodiment of the disclosure. The intelligent container transfer vehicle will be described below as an example. In fig. 8, point a is the front axle center point and point B is the rear axle center point. C is a control point of the vehicle. The desire for path tracking is to have the control point C travel along the desired path. The distances from the point C to the point A and the point B are l respectively f And l r 。l fr =l f +l r ,l fr Is the wheelbase of the vehicle. Delta f Is the equivalent front wheel deflection angle delta of the front axle r Is the equivalent front wheel deflection angle of the rear axle, and ψ is the heading angle of the vehicle. When the vehicle rotates, the points A, B and C rotate around the same circle center O. Vf is the speed of point a, vr is the speed of point B, V is the speed of point C, and β is the slip angle of point C.
Considering that the running speed of the container intelligent transfer vehicle in the working mode is low and is generally not more than 30km/h, the turning radius of the vehicle running path changes slowly at the moment, the change rate of the course angle of the vehicle can be assumed to be equal to the angular speed of the vehicle, namely
Figure SMS_24
Under the inertial coordinate system XY, the kinematic model of the vehicle is:
Figure SMS_25
wherein, (x) C ,y C ) Is the horizontal and vertical coordinates of the C point in an inertial coordinate system.
The vehicle slip angle β can be expressed as:
Figure SMS_26
and then, based on the formulas (1) and (2), carrying out equivalent assumption and simplification by combining the motion characteristics of the intelligent transportation vehicle of the container. The following may be specifically included:
first, a vehicle motion model corresponding to a front axle steering mode
For the intelligent container transfer vehicle in front axle steering mode, the rear axle is equivalent to the front wheel deflection angle delta r Is 0. The motion of the center B point of the rear axle can be described by an Ackerman steering model:
Figure SMS_27
Among these, the ackerman steering model is a common model describing the movement of an unmanned vehicle.
At this time, the front axle center A of the container intelligent transfer vehicle is the front wheel center of the ackerman steering model, and the rear axle center B of the container intelligent transfer vehicle is the rear wheel center of the ackerman steering model. Front axle equivalent front wheel deflection delta of intelligent container transfer vehicle f The front wheel deflection angle of the ackerman steering model is obtained.
Secondly, a vehicle motion model corresponding to a rear axle steering mode
For a container intelligent transfer vehicle in a rear axle steering mode, a front axle is equivalent to a front wheel deflection angle delta f Is 0. At this time, the motion of the center A point of the front axle can still be described by an Ackerman steering model.
Figure SMS_28
Wherein ψ' =ψ+pi.
Compared with the front axle steering mode, the vehicle pose updating flow under the rear axle steering mode is as follows: firstly, after increasing the heading psi of the vehicle by pi degrees, setting to [0,2 pi ]; then, updating the vehicle pose according to the formula (4); finally, updated vehicle heading. The updated heading of the vehicle is: reduced by pi degrees and set to [0,2 pi ].
At this time, the center B point of the rear axle of the container intelligent transfer vehicle is the center of the front wheel of the ackerman steering model, and the center A point of the front axle of the container intelligent transfer vehicle is the center of the rear wheel of the ackerman steering model. Rear axle equivalent front wheel deflection delta of intelligent container transfer vehicle f The front wheel deflection angle of the ackerman steering model is obtained.
Third, vehicle motion model corresponding to double-axle steering mode
Fig. 9 is a schematic diagram of steering motion of a vehicle in a dual-bridge mode according to an embodiment of the disclosure. Referring to fig. 9, in the double-axle steering mode, the front and rear axles are set to have the same steering amplitude and opposite directions, i.e. delta f =-δ r . The control point C is the center point of the wheelbase of the vehicle, i.e. l f =l r =l fr And/2, the vehicle slip angle β is 0 according to the formula (2).
At this time, the double-axle steering motion can be equivalently an ackerman steering model with a wheel base half that of the original vehicle, and the motion at the point C can be described by the ackerman steering model.
Figure SMS_29
At this time, the center point A of the front axle of the vehicle is the center of the front wheel of the ackerman steering model, and the center point C of the wheelbase of the vehicle is the center of the rear wheel of the ackerman steering model. The front axle equivalent front wheel deflection angle δf of the vehicle is the front wheel deflection angle of the ackerman steering model.
Therefore, the vehicle motion model in different steering modes is equivalently simplified into an Ackerman steering model by reasonably supposing the vehicle motion in different steering modes. The equivalent simplified ackerman steering model is used for controlling the vehicle, and for example, the calculated amount can be simplified when path tracking is performed.
On the basis of the above technical solutions, optionally, the method further includes: and sending the front wheel control quantity of the vehicle to a whole vehicle controller so as to control the vehicle to run.
It should be noted that, for simplicity of description, the foregoing method embodiments are all described as a series of acts, but it should be understood by those skilled in the art that the present invention is not limited by the order of acts described, as some steps may be performed in other orders or concurrently in accordance with the present invention. Further, those skilled in the art will also appreciate that the embodiments described in the specification are all preferred embodiments, and that the acts and modules referred to are not necessarily required for the present invention.
Fig. 10 is a schematic structural diagram of a path tracking device in an embodiment of the disclosure. Referring to fig. 10, the path tracking apparatus specifically includes:
a first acquisition module 310 for acquiring a current speed and a current front wheel slip angle of the vehicle;
the pose estimating module 320 is configured to determine, based on a motion model of the vehicle, an estimated pose of the vehicle at a preset moment according to a current speed and a current front wheel yaw angle of the vehicle, where the estimated pose includes an estimated position coordinate of a rear axle center of the motion model;
A second obtaining module 330, configured to obtain, according to the estimated pose and the estimated position coordinate of the center of the rear axle of the motion model, a projection point of the center of the rear axle of the motion model on a desired path, and the feedforward curvature radius and the feedforward heading at the projection point;
a reference circle generating module 340, configured to generate a reference circle according to the feedforward curvature radius and the feedforward heading, where the reference circle is tangential to the desired path at the projection point;
a pretightening point determining module 350, configured to determine a position coordinate of a pretightening point on the reference circle according to a predicted pose of the vehicle after a preset time and a preset pretightening distance;
a control amount determining module 360, configured to determine a front wheel control amount of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
Further, the pose estimation module is used for:
calculating the instantaneous turning radius of the vehicle and the circle center coordinates of the turning position according to the current front wheel deflection angle and the current position coordinates of the rear axle center of the motion model;
calculating a vehicle course angle of the vehicle after a preset moment according to the instantaneous turning radius and the current speed;
And calculating the estimated position coordinate of the rear axle center of the motion model after the preset moment according to the center coordinates of the turning part and the vehicle course angle of the vehicle after the preset moment based on the motion model.
Further, a second acquisition module is configured to:
calculating the distance from each discrete road point on the expected path to the position coordinate of the rear axle center of the motion model after the preset moment, and acquiring a road section between the i-th discrete road point and the i+1-th discrete road point which are closest to the distance as a closest road section;
acquiring the distance from a projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point, the feedforward curvature radius at the ith discrete road point and the feedforward heading at the ith discrete road point, and the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point, the feedforward curvature radius at the (i+1) th discrete road point and the feedforward heading at the (i+1) th discrete road point;
acquiring a feedforward curvature radius at the projection point according to the distance from the projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point, the feedforward curvature radius at the ith discrete road point, the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point and the feedforward curvature radius at the (i+1) th discrete road point;
And acquiring the feedforward course at the projection point according to the distance from the projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point and the feedforward course at the ith discrete road point, the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point and the feedforward course at the (i+1) th discrete road point.
Further, the reference circle generating module is used for:
and acquiring the center coordinates of the reference circle according to the position coordinates of the projection point of the rear axle center of the motion model on the nearest road section, the feedforward curvature radius and the feedforward heading, and generating the reference circle by taking the feedforward curvature radius as the radius.
Further, the pre-aiming point determining module is used for:
transforming the center coordinates of the reference circle to the local coordinate system of the estimated pose to obtain the center coordinates of the reference circle in the local coordinate system of the estimated pose;
acquiring a preset pre-aiming distance;
taking the position coordinate of the rear axle center of the motion model after the preset moment as a circle center, and taking the pre-aiming distance as a radius to obtain a pre-aiming circle;
and acquiring the position coordinates of the pre-aiming point based on the reference circle and the pre-aiming circle under the local coordinate system of the pre-estimated pose.
Further, the pre-aiming point determining module is used for:
p d =d const +V t0
wherein said d const Is a preset distance value, V is the current speed of the vehicle, t 0 Is a preset time value.
Further, the pre-aiming point determining module is used for:
when the reference circle under the local coordinate system of the pre-estimated pose and the pre-aiming circle have no intersection point, determining the position coordinate of the pre-aiming point according to the pre-aiming distance;
and when the reference circle under the local coordinate system of the pre-estimated pose and the pre-aiming circle have at least one intersection point, determining the position coordinate of the pre-aiming point according to the pre-aiming distance, the feedforward curvature radius and the circle center coordinate of the reference circle under the local coordinate system of the pre-estimated pose.
Further, the control amount determining module is configured to:
acquiring current position coordinates of a control point of the vehicle;
and determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the current position coordinates of the control point of the vehicle so that the control point of the vehicle reaches the pre-aiming point along a circular arc path.
Further, the control amount determining module is configured to:
determining a front wheel control amount delta of the vehicle according to the following formula f
Figure SMS_30
Wherein a is the equivalent front wheel deflection angle of the rear axle center of the running model of the vehicle, a A An equivalent front wheel slip angle L for a control point of the vehicle A For the distance of the control point of the vehicle from the pre-aiming point, l fr T is the wheelbase of the vehicle d Is the distance from the control point of the vehicle to the center of the rear axle.
Further, the device further comprises a motion model determining module, based on the motion model of the vehicle, before determining the estimated pose of the vehicle after the preset moment according to the current speed and the current front wheel deflection angle of the vehicle, wherein the motion model determining module is used for:
acquiring a vehicle steering working mode of a current road section;
determining a running model of a vehicle of the current road section according to the vehicle steering working mode of the current road section, wherein a motion model of the vehicle is arranged corresponding to the vehicle steering working mode, and the motion model of the vehicle is obtained by simplifying the standard motion model of the vehicle according to characteristic parameters of the vehicle steering working mode.
Further, the motion model determination module is configured to:
determining the road curvature of the current road section;
and when the curvature of the road is greater than or equal to a first threshold value, determining that the vehicle steering working mode of the current road section is a double-axle steering mode, otherwise, determining that the vehicle steering mode of the current road section is a front axle steering mode or a rear axle steering mode.
Further, the motion model determination module is configured to:
before determining the road curvature of the current road section, determining whether a vehicle steering mode is appointed for the current road section in a road network file;
if yes, determining the vehicle steering mode designated for the current road section in the road network file as the vehicle steering mode of the current road section.
Further, if the vehicle steering operation mode of the current road section is the front axle steering mode, the vehicle motion model corresponding to the rear axle center of the vehicle is obtained by simplifying the vehicle motion model according to the vehicle standard kinematics model, which is:
Figure SMS_31
wherein, (x) B ,y B ) Is the position coordinates of the center of the rear axle of the vehicle,
Figure SMS_32
represents the rate of change on the X-axis, +.>
Figure SMS_33
Representing the rate of change on the Y-axis; delta f Is the equivalent front wheel deflection angle of the front axle, < >>
Figure SMS_34
Is the change rate of the heading angle of the vehicle; l (L) fr Is the wheelbase of the vehicle; v is the vehicle speed, whatThe center of the rear axle of the vehicle is used as the center of the rear axle of the motion model of the vehicle;
if the vehicle steering working mode of the current road section is the rear axle steering mode, the vehicle motion model corresponding to the rear axle center of the vehicle is obtained by simplifying according to the vehicle standard kinematics model 5, and is as follows:
Figure SMS_35
wherein, (x) A ,y A ) Is the position coordinates of the center of the front axle of the vehicle,
Figure SMS_36
Represents the rate of change on the X-axis, +.>
Figure SMS_37
Representing the rate of change on the Y-axis; delta r As the equivalent front wheel deflection angle of the rear axle, ψ' =ψ+pi, ψ is the heading angle of the vehicle, +.>
Figure SMS_38
Representing the change rate of the heading angle; l (L) fr Is the wheelbase of the vehicle; v is the vehicle speed, and the front axle center of the vehicle is used as the rear axle center of the vehicle motion model;
if the vehicle steering working mode of the current road section is the double-axle steering mode, the vehicle motion simplified model corresponding to the rear axle center of the vehicle is obtained by simplifying according to the vehicle standard kinematics model, and is as follows:
Figure SMS_39
wherein, (x) C ,y C ) Is the position coordinates of the center of the vehicle wheelbase of the vehicle,
Figure SMS_40
represents the rate of change on the X-axis, +.>
Figure SMS_41
Represented at YRate of change on axis; delta f Is the equivalent front wheel deflection angle of the front axle, < >>
Figure SMS_42
Is the change rate of the heading angle of the vehicle; l (L) fr Is the wheelbase of the vehicle; v is the vehicle speed, and the vehicle wheelbase central point of the vehicle is used as the rear axle center of the vehicle motion model.
Further, the device also comprises a sending module, wherein the sending module is used for:
and sending the front wheel control quantity of the vehicle to a whole vehicle controller so as to control the vehicle to run.
The path tracking device provided by the embodiment of the present disclosure may implement each step in the path tracking method provided by the embodiment of the present disclosure, and has the same or corresponding beneficial effects, which are not described herein again.
Fig. 11 is a schematic structural diagram of an electronic device in an embodiment of the disclosure. Referring now in particular to fig. 11, a schematic diagram of a configuration of an electronic device 1000 suitable for use in implementing embodiments of the present disclosure is shown. The electronic device 1000 in the embodiments of the present disclosure may include, but is not limited to, mobile terminals such as mobile phones, notebook computers, digital broadcast receivers, PDAs (personal digital assistants), PADs (tablet computers), PMPs (portable multimedia players), in-vehicle terminals (e.g., in-vehicle navigation terminals), wearable electronic devices, and the like, and fixed terminals such as digital TVs, desktop computers, smart home devices, and the like. The electronic device shown in fig. 11 is merely an example, and should not impose any limitations on the functionality and scope of use of embodiments of the present disclosure.
As shown in fig. 11, the electronic device 1000 may include a processing means (e.g., a central processing unit, a graphic processor, etc.) 1001 that may perform various suitable actions and processes according to a program stored in a Read Only Memory (ROM) 1002 or a program loaded from a storage means 1008 into a Random Access Memory (RAM) 1003 to implement a path tracing method of an embodiment as described in the present disclosure. In the RAM 1003, various programs and information necessary for the operation of the electronic apparatus 1000 are also stored. The processing device 1001, the ROM 1002, and the RAM 1003 are connected to each other by a bus 1004. An input/output (I/O) interface 1005 is also connected to bus 1004.
In general, the following devices may be connected to the I/O interface 1005: input devices 1006 including, for example, a touch screen, touchpad, keyboard, mouse, camera, microphone, accelerometer, gyroscope, and the like; an output device 1007 including, for example, a Liquid Crystal Display (LCD), speaker, vibrator, etc.; storage 1008 including, for example, magnetic tape, hard disk, etc.; and communication means 1009. The communication means 1009 may allow the electronic device 1000 to communicate wirelessly or by wire with other devices to exchange information. While fig. 11 shows an electronic device 1000 having various means, it is to be understood that not all of the illustrated means are required to be implemented or provided. More or fewer devices may be implemented or provided instead.
In particular, according to embodiments of the present disclosure, the processes described above with reference to flowcharts may be implemented as computer software programs. For example, embodiments of the present disclosure include a computer program product comprising a computer program embodied on a non-transitory computer readable medium, the computer program comprising program code for performing the method shown in the flowcharts, thereby implementing the path tracking method as described above. In such an embodiment, the computer program may be downloaded and installed from a network via the communication device 1009, or installed from the storage device 1008, or installed from the ROM 1002. The above-described functions defined in the method of the embodiment of the present disclosure are performed when the computer program is executed by the processing device 1001.
It should be noted that the computer readable medium described in the present disclosure may be a computer readable signal medium or a computer readable storage medium, or any combination of the two. The computer readable storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of the computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the context of this disclosure, a computer-readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. In the present disclosure, however, the computer-readable signal medium may include an information signal propagated in baseband or as part of a carrier wave, with the computer-readable program code embodied therein. Such a propagated signal may take many forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to: electrical wires, fiber optic cables, RF (radio frequency), and the like, or any suitable combination of the foregoing.
In some implementations, the clients, servers may communicate using any known or future developed network protocol, such as HTTP (HyperText Transfer Protocol ), and may be interconnected with digital information communication (e.g., a communication network) in any form or medium. Examples of communication networks include a local area network ("LAN"), a wide area network ("WAN"), the internet (e.g., the internet), and peer-to-peer networks (e.g., ad hoc peer-to-peer networks), as well as any known or future developed networks.
The computer readable medium may be contained in the electronic device; or may exist alone without being incorporated into the electronic device.
The computer readable medium carries one or more programs which, when executed by the electronic device, cause the electronic device to:
acquiring the current speed and the current front wheel deflection angle of the vehicle;
determining an estimated pose of the vehicle after a preset moment based on a motion model of the vehicle according to the current speed and the current front wheel deflection angle of the vehicle, wherein the estimated pose comprises an estimated position coordinate of a rear axle center of the motion model;
According to the estimated pose and the estimated position coordinates of the rear axle center of the motion model, a projection point of the rear axle center of the motion model on a desired path, and the feedforward curvature radius and the feedforward heading at the projection point are obtained;
generating a reference circle according to the feedforward curvature radius and the feedforward heading, wherein the reference circle is tangential to the expected path at the projection point;
determining the position coordinates of a pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle at the preset moment and the preset pre-aiming distance;
and determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
Alternatively, the computer-readable medium carries one or more programs that, when executed by the electronic device, cause the electronic device to:
adjusting a control point of the vehicle according to a road parameter of the vehicle for pre-running and/or a size parameter of the vehicle;
the front wheel control amount of the vehicle is calculated based on the adjusted control point of the vehicle.
Alternatively, the electronic device may perform other steps described in the above embodiments when the above one or more programs are executed by the electronic device.
Computer program code for carrying out operations of the present disclosure may be written in one or more programming languages, including, but not limited to, an object oriented programming language such as Java, smalltalk, C ++ and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computer (for example, through the Internet using an Internet service provider).
The units involved in the embodiments of the present disclosure may be implemented by means of software, or may be implemented by means of hardware. Wherein the names of the units do not constitute a limitation of the units themselves in some cases.
The functions described above herein may be performed, at least in part, by one or more hardware logic components. For example, without limitation, exemplary types of hardware logic components that may be used include: a Field Programmable Gate Array (FPGA), an Application Specific Integrated Circuit (ASIC), an Application Specific Standard Product (ASSP), a system on a chip (SOC), a Complex Programmable Logic Device (CPLD), and the like.
In the context of this disclosure, a machine-readable medium may be a tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. The machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium.
According to one or more embodiments of the present disclosure, the present disclosure provides an electronic device comprising:
one or more processors;
a memory for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement any of the path tracking methods as provided by the present disclosure.
According to one or more embodiments of the present disclosure, there is provided a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements a path tracking method as any one of the present disclosure provides.
The disclosed embodiments also provide a computer program product comprising a computer program or instructions which, when executed by a processor, implements the path tracking method as described above.
Scheme 1, a path tracking method, comprising:
acquiring the current speed and the current front wheel deflection angle of the vehicle;
determining an estimated pose of the vehicle after a preset moment based on a motion model of the vehicle according to the current speed and the current front wheel deflection angle of the vehicle, wherein the estimated pose comprises an estimated position coordinate of a rear axle center of the motion model;
according to the estimated pose and the estimated position coordinates of the rear axle center of the motion model, a projection point of the rear axle center of the motion model on a desired path, and the feedforward curvature radius and the feedforward heading at the projection point are obtained;
generating a reference circle according to the feedforward curvature radius and the feedforward heading, wherein the reference circle is tangential to the expected path at the projection point;
determining the position coordinates of a pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle at the preset moment and the preset pre-aiming distance;
And determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
The method according to claim 2, wherein the determining, based on the motion model of the vehicle according to the current speed of the vehicle and the current front wheel slip angle, the estimated pose of the vehicle after the preset time includes:
calculating the instantaneous turning radius of the vehicle and the circle center coordinates of the turning position according to the current front wheel deflection angle and the current position coordinates of the rear axle center of the motion model;
calculating a vehicle course angle of the vehicle after a preset moment according to the instantaneous turning radius and the current speed;
and calculating the estimated position coordinate of the rear axle center of the motion model after the preset moment according to the center coordinates of the turning part and the vehicle course angle of the vehicle after the preset moment based on the motion model.
The method according to the scheme 3 and the scheme 2, wherein the obtaining, according to the estimated pose and the estimated position coordinate of the rear axle center of the motion model, the feedforward curvature radius and the feedforward heading of the rear axle center of the motion model at the projection point on the expected path includes:
calculating the distance from each discrete road point on the expected path to the position coordinate of the rear axle center of the motion model after the preset moment, and acquiring a road section between the i-th discrete road point and the i+1-th discrete road point which are closest to the distance as a closest road section;
Acquiring the distance from a projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point, the feedforward curvature radius at the ith discrete road point and the feedforward heading at the ith discrete road point, and the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point, the feedforward curvature radius at the (i+1) th discrete road point and the feedforward heading at the (i+1) th discrete road point;
acquiring a feedforward curvature radius at the projection point according to the distance from the projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point, the feedforward curvature radius at the ith discrete road point, the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point and the feedforward curvature radius at the (i+1) th discrete road point;
and acquiring the feedforward course at the projection point according to the distance from the projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point and the feedforward course at the ith discrete road point, the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point and the feedforward course at the (i+1) th discrete road point.
Solution 4, the method according to solution 3, the generating a reference circle according to a feedforward curvature radius and the feedforward heading, the reference circle being tangent to the desired path at the projection point, includes:
and acquiring the center coordinates of the reference circle according to the position coordinates of the projection point of the rear axle center of the motion model on the nearest road section, the feedforward curvature radius and the feedforward heading, and generating the reference circle by taking the feedforward curvature radius as the radius.
The method according to the scheme 5 and the scheme 4, wherein the determining the position coordinates of the pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle after the preset moment and the preset pre-aiming distance includes:
transforming the center coordinates of the reference circle to the local coordinate system of the estimated pose to obtain the center coordinates of the reference circle in the local coordinate system of the estimated pose;
acquiring a preset pre-aiming distance;
taking the position coordinate of the rear axle center of the motion model after the preset moment as a circle center, and taking the pre-aiming distance as a radius to obtain a pre-aiming circle;
and acquiring the position coordinates of the pre-aiming point based on the reference circle and the pre-aiming circle under the local coordinate system of the pre-estimated pose.
Solution 6, the method according to solution 5, wherein the preset pretightening distance p is obtained d Comprising:
p d =d const +Vt 0
wherein said d const Is a preset distance value, V is the current speed of the vehicle, t 0 Is a preset time value.
The method according to claim 7, according to claim 5, wherein the obtaining, based on the reference circle and the pretightening circle in the local coordinate system of the estimated pose, the position coordinates of the pretightening point includes:
when the reference circle under the local coordinate system of the pre-estimated pose and the pre-aiming circle have no intersection point, determining the position coordinate of the pre-aiming point according to the pre-aiming distance;
and when the reference circle under the local coordinate system of the pre-estimated pose and the pre-aiming circle have at least one intersection point, determining the position coordinate of the pre-aiming point according to the pre-aiming distance, the feedforward curvature radius and the circle center coordinate of the reference circle under the local coordinate system of the pre-estimated pose.
The method according to claim 8, wherein the determining the front wheel control amount of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle includes:
acquiring current position coordinates of a control point of the vehicle;
and determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the current position coordinates of the control point of the vehicle so that the control point of the vehicle reaches the pre-aiming point along a circular arc path.
The method according to claim 9, according to claim 8, determines a front wheel control amount of the vehicle from the position coordinates of the pre-aiming point and the current position coordinates of the control point of the vehicle, including:
determining a front wheel control amount delta of the vehicle according to the following formula f
Figure SMS_43
Wherein a is the equivalent front wheel deflection angle of the rear axle center of the running model of the vehicle, a A An equivalent front wheel slip angle L for a control point of the vehicle A For the distance of the control point of the vehicle from the pre-aiming point, l fr T is the wheelbase of the vehicle d Is the distance from the control point of the vehicle to the center of the rear axle.
The method according to claim 10 and claim 1, wherein the determining the estimated pose of the vehicle after the preset time based on the motion model of the vehicle according to the current speed and the current front wheel bias angle of the vehicle further includes:
acquiring a vehicle steering working mode of a current road section;
determining a running model of a vehicle of the current road section according to the vehicle steering working mode of the current road section, wherein a motion model of the vehicle is arranged corresponding to the vehicle steering working mode, and the motion model of the vehicle is obtained by simplifying the standard motion model of the vehicle according to characteristic parameters of the vehicle steering working mode.
The method according to claim 11, wherein the obtaining the vehicle steering operation mode of the current road segment includes:
determining the road curvature of the current road section;
and when the curvature of the road is greater than or equal to a first threshold value, determining that the vehicle steering working mode of the current road section is a double-axle steering mode, otherwise, determining that the vehicle steering mode of the current road section is a front axle steering mode or a rear axle steering mode.
Solution 12, the method according to solution 11, before determining the road curvature of the current road segment, further includes:
determining whether a vehicle steering mode is appointed for a current road section in the road network file;
if yes, determining the vehicle steering mode designated for the current road section in the road network file as the vehicle steering mode of the current road section.
According to the method of the scheme 13, according to the scheme 11, if the vehicle steering operation mode of the current road section is the front axle steering mode, the vehicle motion model corresponding to the rear axle center of the vehicle is obtained by simplifying according to the vehicle standard kinematics model, which is:
Figure SMS_44
wherein, (x) B ,y B ) Is the position coordinates of the center of the rear axle of the vehicle,
Figure SMS_45
represents the rate of change on the X-axis, +.>
Figure SMS_46
Representation ofRate of change on the Y axis; delta f Is the equivalent front wheel deflection angle of the front axle, < > >
Figure SMS_47
Is the change rate of the heading angle of the vehicle; l (L) fr Is the wheelbase of the vehicle; v is the speed of the vehicle, and the center of the rear axle of the vehicle is used as the center of the rear axle of the motion model of the vehicle;
if the vehicle steering working mode of the current road section is the rear axle steering mode, the vehicle motion model corresponding to the rear axle center of the vehicle is obtained by simplifying the vehicle steering working mode according to the vehicle standard kinematics model, and is as follows:
Figure SMS_48
wherein, (x) A ,y A ) Is the position coordinates of the center of the front axle of the vehicle,
Figure SMS_49
represents the rate of change on the X-axis, +.>
Figure SMS_50
Representing the rate of change on the Y-axis; delta r As the equivalent front wheel deflection angle of the rear axle, ψ' =ψ+pi, ψ is the heading angle of the vehicle, +.>
Figure SMS_51
Representing the change rate of the heading angle; l (L) fr Is the wheelbase of the vehicle; v is the vehicle speed, and the front axle center of the vehicle is used as the rear axle center of the vehicle motion model;
if the vehicle steering working mode of the current road section is the double-axle steering mode, the vehicle motion simplified model corresponding to the rear axle center of the vehicle is obtained by simplifying according to the vehicle standard kinematics model, and is as follows:
Figure SMS_52
wherein, (x) C ,y C ) Position coordinates of the centre of the wheelbase of a vehicle,
Figure SMS_53
Represents the rate of change on the X-axis, +.>
Figure SMS_54
Representing the rate of change on the Y-axis; delta f Is equivalent front wheel deflection angle of front axle +. >
Figure SMS_55
Is the change rate of the heading angle of the vehicle; l (L) fr Is the wheelbase of the vehicle; v is the vehicle speed, and the vehicle wheelbase central point of the vehicle is used as the rear axle center of the vehicle motion model.
Scheme 14, the method of scheme 1, further comprising:
and sending the front wheel control quantity of the vehicle to a whole vehicle controller so as to control the vehicle to run.
Scheme 15, a path tracking device, comprising:
the first acquisition module is used for acquiring the current speed and the current front wheel deflection angle of the vehicle;
the pose estimating module is used for determining an estimated pose of the vehicle after a preset moment based on a motion model of the vehicle according to the current speed and the current front wheel deflection angle of the vehicle, wherein the estimated pose comprises an estimated position coordinate of a rear axle center of the motion model;
the second acquisition module is used for acquiring a projection point of the rear axle center of the motion model on a desired path, and the feedforward curvature radius and the feedforward heading at the projection point according to the estimated pose and the estimated position coordinate of the rear axle center of the motion model;
the reference circle generation module is used for generating a reference circle according to the feedforward curvature radius and the feedforward heading, and the reference circle is tangential to the expected path at the projection point;
The pre-aiming point determining module is used for determining the position coordinates of the pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle after the preset moment and the preset pre-aiming distance;
and the control quantity determining module is used for determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
Scheme 16, an electronic device, the electronic device comprising:
one or more processors;
a storage means for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the methods of any of aspects 1-14.
Aspect 17, a computer readable storage medium having stored thereon a computer program which when executed by a processor implements the method of any of aspects 1-14.
It should be noted that the terms "first," "second," and the like in this disclosure are merely used to distinguish between different devices, modules, or units and are not used to define an order or interdependence of functions performed by the devices, modules, or units.
The foregoing description is only of the preferred embodiments of the present disclosure and description of the principles of the technology being employed. It will be appreciated by persons skilled in the art that the scope of the disclosure referred to in this disclosure is not limited to the specific combinations of features described above, but also covers other embodiments which may be formed by any combination of features described above or equivalents thereof without departing from the spirit of the disclosure. Such as those described above, are mutually substituted with the technical features having similar functions disclosed in the present disclosure (but not limited thereto).

Claims (10)

1. A method of path tracking, comprising:
acquiring the current speed and the current front wheel deflection angle of the vehicle;
determining an estimated pose of the vehicle after a preset moment based on a motion model of the vehicle according to the current speed and the current front wheel deflection angle of the vehicle, wherein the estimated pose comprises an estimated position coordinate of a rear axle center of the motion model;
according to the estimated pose and the estimated position coordinates of the rear axle center of the motion model, a projection point of the rear axle center of the motion model on a desired path, and the feedforward curvature radius and the feedforward heading at the projection point are obtained;
generating a reference circle according to the feedforward curvature radius and the feedforward heading, wherein the reference circle is tangential to the expected path at the projection point;
determining the position coordinates of a pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle at the preset moment and the preset pre-aiming distance;
and determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
2. The method of claim 1, wherein the determining the estimated pose of the vehicle after the preset moment based on the motion model of the vehicle according to the current speed of the vehicle and the current front wheel bias angle comprises:
Calculating the instantaneous turning radius of the vehicle and the circle center coordinates of the turning position according to the current front wheel deflection angle and the current position coordinates of the rear axle center of the motion model;
calculating a vehicle course angle of the vehicle after a preset moment according to the instantaneous turning radius and the current speed;
and calculating the estimated position coordinate of the rear axle center of the motion model after the preset moment according to the center coordinates of the turning part and the vehicle course angle of the vehicle after the preset moment based on the motion model.
3. The method according to claim 2, wherein the obtaining the feedforward curvature radius and the feedforward heading of the rear axle center of the motion model at the projection point on the desired path according to the estimated pose and the estimated position coordinates of the rear axle center of the motion model comprises:
calculating the distance from each discrete road point on the expected path to the position coordinate of the rear axle center of the motion model after the preset moment, and acquiring a road section between the i-th discrete road point and the i+1-th discrete road point which are closest to the distance as a closest road section;
acquiring the distance from a projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point, the feedforward curvature radius at the ith discrete road point and the feedforward heading at the ith discrete road point, and the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point, the feedforward curvature radius at the (i+1) th discrete road point and the feedforward heading at the (i+1) th discrete road point;
Acquiring a feedforward curvature radius at the projection point according to the distance from the projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point, the feedforward curvature radius at the ith discrete road point, the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point and the feedforward curvature radius at the (i+1) th discrete road point;
and acquiring the feedforward course at the projection point according to the distance from the projection point of the rear axle center of the motion model on the nearest road section to the ith discrete road point and the feedforward course at the ith discrete road point, the distance from the projection point of the rear axle center of the motion model on the nearest road section to the (i+1) th discrete road point and the feedforward course at the (i+1) th discrete road point.
4. A method according to claim 3, wherein said generating a reference circle from a feedforward radius of curvature and the feedforward heading, the reference circle being tangent to the desired path at the projection point, comprises:
and acquiring the center coordinates of the reference circle according to the position coordinates of the projection point of the rear axle center of the motion model on the nearest road section, the feedforward curvature radius and the feedforward heading, and generating the reference circle by taking the feedforward curvature radius as the radius.
5. The method according to claim 4, wherein the determining the position coordinates of the pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle after the preset time and the pre-set pre-aiming distance includes:
transforming the center coordinates of the reference circle to the local coordinate system of the estimated pose to obtain the center coordinates of the reference circle in the local coordinate system of the estimated pose;
acquiring a preset pre-aiming distance;
taking the position coordinate of the rear axle center of the motion model after the preset moment as a circle center, and taking the pre-aiming distance as a radius to obtain a pre-aiming circle;
and acquiring the position coordinates of the pre-aiming point based on the reference circle and the pre-aiming circle under the local coordinate system of the pre-estimated pose.
6. The method according to claim 5, wherein the predetermined pretightening distance p is obtained d Comprising:
p d =d const +Vt 0
wherein said d const Is a preset distance value, V is the current speed of the vehicle, t 0 Is a preset time value.
7. The method of claim 5, wherein the obtaining the position coordinates of the pre-aiming point based on the reference circle and the pre-aiming circle in the local coordinate system of the pre-estimated pose comprises:
when the reference circle under the local coordinate system of the pre-estimated pose and the pre-aiming circle have no intersection point, determining the position coordinate of the pre-aiming point according to the pre-aiming distance;
And when the reference circle under the local coordinate system of the pre-estimated pose and the pre-aiming circle have at least one intersection point, determining the position coordinate of the pre-aiming point according to the pre-aiming distance, the feedforward curvature radius and the circle center coordinate of the reference circle under the local coordinate system of the pre-estimated pose.
8. A path tracking device, comprising:
the first acquisition module is used for acquiring the current speed and the current front wheel deflection angle of the vehicle;
the pose estimating module is used for determining an estimated pose of the vehicle after a preset moment based on a motion model of the vehicle according to the current speed and the current front wheel deflection angle of the vehicle, wherein the estimated pose comprises an estimated position coordinate of a rear axle center of the motion model;
the second acquisition module is used for acquiring a projection point of the rear axle center of the motion model on a desired path, and the feedforward curvature radius and the feedforward heading at the projection point according to the estimated pose and the estimated position coordinate of the rear axle center of the motion model;
the reference circle generation module is used for generating a reference circle according to the feedforward curvature radius and the feedforward heading, and the reference circle is tangential to the expected path at the projection point;
The pre-aiming point determining module is used for determining the position coordinates of the pre-aiming point on the reference circle according to the pre-estimated pose of the vehicle after the preset moment and the preset pre-aiming distance;
and the control quantity determining module is used for determining the front wheel control quantity of the vehicle according to the position coordinates of the pre-aiming point and the control point of the vehicle.
9. An electronic device, the electronic device comprising:
one or more processors;
a storage means for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of any of claims 1-7.
10. A computer readable storage medium, on which a computer program is stored, characterized in that the program, when being executed by a processor, implements the method according to any of claims 1-7.
CN202211677036.4A 2022-12-26 2022-12-26 Path tracking method, path tracking device, electronic equipment and storage medium Pending CN116414120A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211677036.4A CN116414120A (en) 2022-12-26 2022-12-26 Path tracking method, path tracking device, electronic equipment and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211677036.4A CN116414120A (en) 2022-12-26 2022-12-26 Path tracking method, path tracking device, electronic equipment and storage medium

Publications (1)

Publication Number Publication Date
CN116414120A true CN116414120A (en) 2023-07-11

Family

ID=87048691

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211677036.4A Pending CN116414120A (en) 2022-12-26 2022-12-26 Path tracking method, path tracking device, electronic equipment and storage medium

Country Status (1)

Country Link
CN (1) CN116414120A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118153299A (en) * 2024-03-07 2024-06-07 北京易航远智科技有限公司 Vehicle location information determination method, electronic device, device and storage medium

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118153299A (en) * 2024-03-07 2024-06-07 北京易航远智科技有限公司 Vehicle location information determination method, electronic device, device and storage medium

Similar Documents

Publication Publication Date Title
CN110096053B (en) Driving trajectory generation method, system and machine-readable medium for autonomous vehicle
CN109799828B (en) Transverse and longitudinal coordination control method for automatic driving vehicle
CN112051864B (en) Method, apparatus, device and readable medium for tracking moving target trajectory
WO2023142794A1 (en) Vehicle control method and apparatus, and device and storage medium
CN110795523A (en) Vehicle positioning method and device and intelligent vehicle
CN113778045A (en) Method and device for evaluating automatic driving vehicle controller
CN111796521A (en) Foresight distance determining method, device, equipment and storage medium
CN112590813A (en) Method, apparatus, electronic device, and medium for generating information of autonomous vehicle
WO2022247203A1 (en) Method and apparatus for controlling autonomous vehicle, device, and storage medium
CN112009460B (en) Vehicle control method, device, equipment and storage medium
CN110231041A (en) A kind of air navigation aid and device of lane switching
CN115534925A (en) Vehicle control method, device, equipment and computer readable medium
CN112256006A (en) A data processing method, device and electronic device
CN111338339B (en) Track planning method, track planning device, electronic equipment and computer readable medium
CN113805578B (en) Unmanned vehicle path optimization method and related equipment
CN116414120A (en) Path tracking method, path tracking device, electronic equipment and storage medium
CN117584972A (en) Vehicle transverse control method and device, electronic equipment and medium
CN113743456A (en) Scene positioning method and system based on unsupervised learning
CN111469781A (en) Method and apparatus for outputting information
CN113306570B (en) Method and device for controlling an autonomous vehicle and autonomous dispensing vehicle
CN111098842A (en) Vehicle speed control method and related equipment
CN115098821A (en) Trajectory reference curvature determination method, apparatus, device, medium, and program
CN111443698A (en) Posture self-adjusting mobile balancing device and method, electronic terminal and storage medium
CN117928575A (en) Lane information extraction method, system, electronic device and storage medium
CN115542899B (en) Vehicle path tracking method, device, vehicle, electronic device and medium

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