CN112172800B - Articulated vehicle and tracking parking control method and device thereof - Google Patents
Articulated vehicle and tracking parking control method and device thereof Download PDFInfo
- Publication number
- CN112172800B CN112172800B CN202011351378.8A CN202011351378A CN112172800B CN 112172800 B CN112172800 B CN 112172800B CN 202011351378 A CN202011351378 A CN 202011351378A CN 112172800 B CN112172800 B CN 112172800B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- tail
- point
- head
- speed
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 230000003137 locomotive effect Effects 0.000 claims description 25
- 239000000126 substance Substances 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 5
- 238000004590 computer program Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 5
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/06—Automatic manoeuvring for parking
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
The invention provides an articulated vehicle and a tracking parking control method and device thereof, wherein the method comprises the following steps: acquiring a mass center parameter of the vehicle head; acquiring parameters of a head hinge point according to the mass center parameters of the head; acquiring parameters of a vehicle tail hinge point according to the parameters of the vehicle head hinge point; acquiring a barycenter parameter of the tail of the vehicle according to the parameters of the tail hinge point of the vehicle; calculating a steering instruction according to the barycenter parameter of the tail of the vehicle and a preset reference point parameter; and controlling the steering of the vehicle according to the steering command, and controlling the vehicle to stop when the vehicle position reaches a stop point. The tracking parking control method can smoothly park the vehicle, has small control error and realizes accurate parking position.
Description
Technical Field
The invention relates to the technical field of articulated vehicles, in particular to a method for controlling articulated vehicle tracking parking, a device for controlling articulated vehicle tracking parking and an articulated vehicle.
Background
The prior art relates to a method of assisting the backing of an articulated vehicle, comprising the steps of: recording a predetermined number of positions of the first articulated vehicle for the specified path; recording articulation angles of each articulation joint of the articulated vehicle at a predetermined number of positions; recording the orientation of the first articulated vehicle at a predetermined number of positions; saving the record value of the designated path in a memory; calculating a swept area of the first articulated vehicle for the specified path by using the recorded values and the size information of the articulated vehicle; and controlling steering of the articulated vehicle using the swept area when the articulated vehicle is reversing along the specified path such that the articulated vehicle does not extend outside the swept area during reversing. According to the technical scheme, the hinge angles of the positions with the preset number need to be recorded in advance, the connection angle is not easy to measure, the operation is complex, the labor cost is high, and the method is not suitable for batch application.
Disclosure of Invention
The invention aims to solve the technical problems and provides a tracking parking control method for an articulated vehicle, which can smoothly park the vehicle, has small control error and can accurately reach a parking position.
The technical scheme adopted by the invention is as follows:
a method of controlling tracked parking of an articulated vehicle, the method comprising the steps of: acquiring a mass center parameter of the vehicle head; acquiring parameters of a head hinge point according to the mass center parameters of the head; acquiring parameters of a vehicle tail hinge point according to the parameters of the vehicle head hinge point; acquiring a barycenter parameter of the tail of the vehicle according to the parameters of the tail hinging point; calculating a steering instruction according to the barycenter parameter of the tail of the vehicle and a preset reference point parameter; and controlling the steering of the vehicle according to the steering command, and controlling the vehicle to stop when the vehicle position reaches a stopping point.
According to one embodiment of the present invention, the centroid parameter of the vehicle head, the vehicle head hinge point, the vehicle tail hinge point and the centroid parameter of the vehicle tail respectively comprise: position, velocity, attitude, and attitude angular velocity, wherein the attitude comprises: a heading angle, the attitude angular velocity comprising: course angular velocity, according to the barycenter parameter of locomotive obtain the parameter of locomotive pin joint, include: converting the position and the speed of the mass center of the vehicle head by adopting a lever arm compensation mode to obtain the position and the speed of a hinge point of the vehicle head; and taking the attitude and the attitude angular velocity of the center of mass of the headstock as the attitude and the attitude angular velocity of the hinging point of the headstock.
According to one embodiment of the invention, acquiring the parameters of the car tail hinge point according to the parameters of the car head hinge point comprises the following steps: acquiring an articulation angle of the vehicle; carrying out difference average filtering on the articulation angle of the vehicle to obtain the articulation angular velocity of the vehicle; acquiring a course angle and a course angular velocity of the vehicle tail hinge point according to the course angle of the vehicle head hinge point, the course angular velocity of the vehicle head hinge point, the hinge angle of the vehicle and the hinge angular velocity of the vehicle; and taking the position and the speed of the head hinge point as the position and the speed of the tail hinge point.
According to an embodiment of the present invention, acquiring the course angle and the course angular velocity of the car tail hinge point according to the course angle of the car head hinge point, the course angular velocity of the car head hinge point, the hinge angle of the car and the hinge angular velocity of the car comprises: taking the sum of the course angle of the vehicle head hinge point and the hinge angle of the vehicle as the course angle of the vehicle tail hinge point; and taking the sum of the course angular speed of the vehicle head hinge joint and the hinge angle speed of the vehicle as the course angular speed of the vehicle tail hinge joint.
According to one embodiment of the invention, the obtaining of the barycenter parameter of the car tail according to the parameters of the car tail hinge point comprises the following steps: converting the position and the speed of the tail hinging point by adopting a lever arm compensation mode to obtain the position and the speed of the tail mass center; and taking the course angle and the course angular speed of the tail hinging point as the course angle and the course angular speed of the tail mass center.
According to one embodiment of the invention, calculating a steering command according to the centroid parameter of the vehicle tail and the preset reference point parameter comprises: calculating errors between the position, the speed, the course angle and the course angular speed of the mass center of the vehicle tail and the position, the speed, the course angle and the course angular speed of the preset reference point respectively; and calculating the steering instruction according to the position error, the speed error, the course angle error and the course angular speed error.
According to one embodiment of the invention, the position error and the velocity error are calculated by the following equations:
wherein the content of the first and second substances,
is indicative of the position error in question,
and
representing the centroid position coordinates of the vehicle tail,
and
a position coordinate representing the preset reference point,
which represents the angle of the heading direction,
indicating the course angle of the preset reference point,
is indicative of the speed error in question,
representing an east velocity of the center of mass of the vehicle tail,
representing an east-direction velocity of the preset reference point,
representing the northbound speed of the center of mass of the vehicle tail,
representing the northbound speed of the preset reference point.
According to one embodiment of the invention, the steering command is calculated by the following formula:
wherein the content of the first and second substances,
a direction of the steering command is indicated,
、
、
and
the number of the symbols representing the constant number,
a heading angular velocity representing the center of mass of the vehicle's tail,
indicating the heading angular velocity of a preset reference point.
The invention also provides a tracking parking control device for the articulated vehicle, which comprises the following components: the first acquisition module is used for acquiring a centroid parameter of the locomotive; the second acquisition module is used for acquiring parameters of a head hinge point according to the mass center parameters of the head; the third acquisition module is used for acquiring parameters of the car tail hinge point according to the parameters of the car head hinge point; the fourth acquisition module is used for acquiring the barycenter parameter of the tail of the vehicle according to the parameters of the tail hinging point; the calculation module is used for calculating a steering instruction according to the barycenter parameter of the vehicle tail and a preset reference point parameter; and the control module is used for controlling the steering of the vehicle according to the steering instruction and controlling the vehicle to stop when the position of the vehicle reaches a stopping point.
The invention also provides an articulated vehicle which comprises the control device for tracking parking of the articulated vehicle.
The invention has the beneficial effects that:
the invention can realize smooth parking through control logic on the basis of the existing sensor on the articulated vehicle, has small control error and realizes accurate arrival at the parking position.
Drawings
FIG. 1 is a flowchart of a method for controlling tracked parking of an articulated vehicle according to an embodiment of the present invention;
FIG. 2 is a schematic view of the translation of the location and velocity of the center of mass of the vehicle head to the articulation point of the vehicle head in accordance with one embodiment of the present invention;
FIG. 3 is a schematic illustration of an articulated vehicle parking according to one embodiment of the present invention;
FIG. 4 is a block diagram of an articulated vehicle tracking parking control apparatus according to an embodiment of the present invention;
fig. 5 is a block schematic diagram of a vehicle according to an embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Fig. 1 is a flowchart of a method for controlling tracking parking of an articulated vehicle according to an embodiment of the present invention.
As shown in fig. 1, a method for controlling tracked parking of an articulated vehicle according to an embodiment of the present invention may include the following steps:
and S1, acquiring the centroid parameters of the vehicle head.
In one embodiment of the invention, the locomotive centroid parameters may include: position, speed, gesture and the gesture angular velocity of locomotive barycenter, wherein, the position of barycenter includes longitude, latitude and height, and speed includes the speed of east, north, three direction in sky, and the gesture can include course angle, angle of pitch and roll angle, and gesture angular velocity includes: course angular velocity, pitch angular velocity, and roll angular velocity. The inertial navigation is installed on the existing vehicle, and the positioning sensor is installed at the position of the head of the vehicle, so that the parameters of the mass center of the head of the vehicle can be obtained.
And S2, acquiring parameters of the head hinge point according to the mass center parameters of the head. Wherein, the parameter of locomotive pin joint includes: the position of locomotive pin joint, the speed of locomotive pin joint, the gesture of locomotive pin joint and the gesture angular velocity of locomotive pin joint.
According to one embodiment of the invention, the obtaining of the parameters of the head hinge point according to the centroid parameters of the head comprises the following steps: converting the position and the speed of the mass center of the vehicle head by adopting a lever arm compensation mode to obtain the position and the speed of a hinge point of the vehicle head; and taking the attitude and the attitude angular velocity of the center of mass of the head as the attitude and the attitude angular velocity of the hinged point of the head.
Particularly, because the locomotive barycenter and the locomotive pin joint are on the same rigid body, therefore need not change gesture and gesture angular velocity, only need change the position and the speed of locomotive barycenter, obtain the position and the speed of locomotive pin joint. That is, the attitude (course angle, pitch angle, roll angle) and the attitude angular velocity (course angular velocity, pitch angular velocity, roll angular velocity) of the centroid of the vehicle head are the attitude and the attitude angular velocity of the vehicle head hinge point.
The position and the speed of the center of mass of the head of the vehicle are converted into the position and the speed of a hinged point of the head of the vehicle by adopting a lever arm compensation mode, and the specific conversion process is as follows:
as shown in FIG. 2, assume that the vector of the locomotive articulation point relative to the earth center O isRThe vector of the centroid of the headstock relative to the centroid O isrThe vector of the head hinge point relative to the mass center of the head is
And the vector relationship among the three satisfies the following conditions:
。
according to the relative position relationship in fig. 1, the position relationship between the head hinge point and the head mass center can be obtained by combining a relative derivation formula:
in the formula (I), the compound is shown in the specification,
，
，
，
，
。
wherein the content of the first and second substances,
the velocity vector of the head articulation point is shown,
a velocity vector representing the centroid of the vehicle head,
is shown in the vehicle system (bRelative to a navigational coordinate system (nIs) of the attitude matrix of the object,
respectively representing a course angle, a pitch angle and a roll angle,
to represent
，
To represent
，
To represent
，
To represent
，
To represent
，
To represent
，
Representing a vector
The corresponding cross-multiplication matrix is then used,
is composed ofbIs relative tonThe angular velocity of the system can be approximated to the gyroscope output of an IMU (Inertial Measurement Unit),
a vector representing the head articulation point relative to the head center of mass,
a position vector of a head hinging point is shown,
position vector representing the centroid of a vehicle head，
A matrix of the parameters is represented and,
a secant function representing the latitude L,
the radius of the meridian main curvature at the position of the height h,
the main curvature radius of the position unitary mortise of the height h,
is the radius of the main curvature of the meridian,
is the main curvature radius of the unitary-mortise ring,Bwhich represents the longitude of the vehicle,arepresents the radius of the long axis of the earth,bwhich represents the short axis radius of the earth,erepresenting the eccentricity of the ellipsoidal surface of the earth.
And S3, acquiring parameters of the car tail hinge point according to the parameters of the car head hinge point.
According to one embodiment of the invention, the acquiring of the parameters of the car tail hinge point according to the parameters of the car head hinge point comprises the following steps: acquiring an articulation angle of the vehicle; carrying out difference average filtering on the articulation angle of the vehicle to obtain the articulation angular velocity of the vehicle; acquiring a course angle and a course angular velocity of a vehicle tail hinge point according to the course angle of the vehicle head hinge point, the course angular velocity of the vehicle head hinge point, the hinge angle of the vehicle and the hinge angular velocity of the vehicle; and taking the position and the speed of the hinging point of the head of the bicycle as the position and the speed of the hinging point of the tail of the bicycle.
Further, according to an embodiment of the present invention, the obtaining the course angle and the course angular velocity of the car tail hinge point according to the course angle of the car head hinge point, the course angular velocity of the car head hinge point, the hinge angle of the car and the hinge angular velocity of the car includes: taking the sum of the course angle of the head hinge point and the hinge angle of the vehicle as the course angle of the tail hinge point; and taking the sum of the course angular speed of the vehicle head hinge point and the hinge angle speed of the vehicle as the course angular speed of the vehicle tail hinge point.
Particularly, because locomotive pin joint and rear of a vehicle pin joint are same point, so the position and the speed of locomotive joint point are position and the speed of rear of a vehicle pin joint promptly, but locomotive joint point fixes in the locomotive portion, and the rear of a vehicle pin joint is fixed in the rear of a vehicle portion, so the course angle and the course angular velocity of locomotive joint point and rear of a vehicle pin joint are different.
The articulation angle is obtained by the articulation angle sensor, such as the angle α in fig. 3, and the obtained articulation angle is subjected to difference mean filtering to obtain an articulation angular velocity ω', and, if so, a series of articulation angle values α are obtained1、α2、α3、α4、α5、α6、α7、α8、α9、α10、α11With a calculation period T, the value ω of the articulation angular velocity can be correspondingly obtained1′=（α2-α1）/T，…，ω10′=（α11-α10) By setting the filter length to 10,/T, the articulation angular velocity ω' = (ω) can be obtained1′+ω2′+…+ω10')/10. It will be appreciated that if the filter length is not sufficient for 10, then the several data obtained are averaged over the several data, the purpose of the filter being to remove differential noise.
Suppose that the heading angle of the head hinge point is
(same as the course angle of the center of mass of the head) and the course angular speed of
(the heading angular velocity is the same as that of the center of mass of the head of the vehicle), and the heading angle of the tail hinging point is
Navigation systemTo an angular velocity of
Then, the course angle of the tail hinge point of the vehicle can be calculated by the following formula
A course angular velocity of
：
，
。
And S4, acquiring the barycenter parameter of the tail of the vehicle according to the parameters of the tail hinge point of the vehicle.
According to one embodiment of the invention, the obtaining of the barycenter parameter of the car tail according to the parameters of the car tail hinge point comprises the following steps: converting the position and the speed of a tail hinging point by adopting a lever arm compensation mode to obtain the position and the speed of a tail mass center; and taking the course angle and the course angular speed of the tail hinging point as the course angle and the course angular speed of the tail mass center.
With the same reasoning of obtaining the hinge point parameter of the vehicle head according to the mass center parameter of the vehicle head in the above embodiment, the mass center parameter of the vehicle tail is obtained according to the parameter of the hinge point of the vehicle tail by utilizing the lever arm compensation mode. The automobile tail hinge point and the automobile tail center of mass are on the same rigid body, so that the posture and the posture angular speed do not need to be converted, and the position and the speed of the automobile tail hinge point only need to be converted. The specific conversion is made with reference to the above-described embodiments, which are not described in detail herein.
And S5, calculating a steering command according to the barycenter parameter of the tail of the vehicle and the preset reference point parameter.
The preset reference point acquisition mode is as follows: in FIG. 3, there are a series of target track points (each reference point information is: position, speed in three directions of northeast, heading angle, heading angular velocity). And converting the position coordinates of the mass center of the tail of the vehicle into a coordinate system which is the same as the reference point, and searching the reference point which is closest to the position coordinates of the mass center of the tail of the vehicle according to the position of the mass center of the tail of the vehicle to be used as a preset reference point.
According to one embodiment of the invention, calculating the steering command according to the centroid parameter of the vehicle tail and the preset reference point parameter comprises: calculating errors between the position, the speed, the course angle and the course angular speed of the mass center of the tail of the vehicle and the position, the speed, the course angle and the course angular speed of a preset reference point respectively; and calculating a steering instruction according to the position error, the speed error, the course angle error and the course angular speed error.
Further, in one embodiment of the present invention, the position error and the velocity error are calculated by the following formulas:
wherein the content of the first and second substances,
the error in the position is indicated by a position error,
and
representing the coordinates of the centroid location of the vehicle tail,
and
a position coordinate representing a preset reference point,
which represents the angle of the heading direction,
indicating the course angle of the preset reference point,
the speed error is indicated in the form of a speed error,
representing the east velocity of the center of mass of the vehicle's tail,
representing the east velocity of a preset reference point,
representing the northbound speed of the center of mass of the vehicle tail,
representing the northbound speed of a preset reference point.
Calculating a steering command by the following formula:
wherein the content of the first and second substances,
a steering command is indicated and the steering is commanded,
、
、
and
the number of the symbols representing the constant number,
the course angular velocity representing the center of mass of the vehicle tail,
indicating the heading angular velocity of a preset reference point.
Specifically, it is assumed that the parameters of the preset reference point include: position coordinates (
，
) East speed
Speed in the north direction
Angle of course
Course angular velocity
The parameters of the center of mass of the vehicle tail are as follows: coordinates of the center of mass (
，
) East speed
Speed in the north direction
Angle of course
Course angular velocity
Then, the lateral error and the lateral velocity of the vehicleThe errors are as follows:
in the formula, in the two formulas
、
The method relates to the conversion of a position vector and a speed vector from a preset reference point to the center of mass of the tail of the vehicle into a preset reference point coordinate system (the origin of coordinates is the preset reference point, the ordinate is the tangential direction of the preset reference point, such as the coordinate system of the preset reference point in fig. 3), and the abscissa is a transverse position error and a transverse speed error. It should be noted that the heading north is 0 degrees, the north is positive when the west is north, and the north is negative when the east is north.
From the lateral position error calculated in real time
Lateral velocity error
Course angle error
Course angular velocity error
. And (3) calculating a steering command by adopting a PD control algorithm through the following formula:
. Wherein the content of the first and second substances,
with a distinction between positive and negative, sign indicating reversal of direction of steering of the vehicle, e.g. when
When the value is negative, the vehicle is controlled to turn left, when
And when the vehicle speed is positive, controlling the vehicle to turn to the right.
It will be appreciated that the retrieved steering commands may also be low pass filtered in order to reduce noise during the several transitions described above.
And S6, controlling the steering of the vehicle according to the steering command, and controlling the vehicle to stop when the vehicle position reaches a stop point.
That is to say, judge the relation between the position of vehicle position and the parking stall in real time, turn to according to steering command control vehicle and turn to, when the vehicle reaches the parking stall, control vehicle parking to need not additionally to increase the positioning sensor, and need not record the articulated angle of predetermined number position in advance, utilize the positioning sensor that current locomotive portion set up, carry out the barycenter parameter of locomotive and the barycenter parameter of rear of a vehicle conversion, realize smooth parking, and control error is little, can accurate arrival parking stall parking.
In conclusion, the control method of the invention obtains the parameters of the head hinge point according to the centroid parameters of the head; acquiring parameters of a vehicle tail hinge point according to the parameters of the vehicle head hinge point; acquiring a barycenter parameter of the tail of the vehicle according to the parameters of the tail hinge point of the vehicle; calculating a steering instruction according to the barycenter parameter of the tail of the vehicle and a preset reference point parameter; and controlling the steering of the vehicle according to the steering command, and controlling the vehicle to stop when the vehicle position reaches a stop point. Therefore, smooth parking is realized, the control error is small, and the accurate parking position is realized.
The invention further provides a tracking parking control device of the articulated vehicle corresponding to the embodiment.
Fig. 4 is a block diagram illustrating an articulated vehicle tracking parking control apparatus according to an embodiment of the present invention.
As shown in fig. 4, the control device for tracking parking of an articulated vehicle according to an embodiment of the present invention may include: a first obtaining module 10, a second obtaining module 20, a third obtaining module 30, a fourth obtaining module 40, a calculating module 50 and a control module 60.
The first obtaining module 10 is configured to obtain a centroid parameter of the vehicle head. The second obtaining module 20 is configured to obtain a parameter of a hinge point of the vehicle head according to a centroid parameter of the vehicle head. The third obtaining module 30 is used for obtaining parameters of the car tail hinge point according to the parameters of the car head hinge point. The fourth obtaining module 40 is configured to obtain a centroid parameter of the car tail according to a parameter of a car tail hinge point. The calculation module 50 is used for calculating a steering instruction according to the barycenter parameter of the vehicle tail and the preset reference point parameter. The control module 60 is configured to control steering of the vehicle according to the steering command, and to control the vehicle to stop when the vehicle position reaches a stop point.
According to one embodiment of the invention, the centroid parameters of the vehicle head, the vehicle head hinge point, the vehicle tail hinge point and the vehicle tail respectively comprise: position, velocity, attitude, and attitude angular velocity, wherein the attitude comprises: the course angle, attitude angular velocity includes: the second acquisition module 20 is used for acquiring parameters of a head hinge point according to the parameters of the mass center of the head, and specifically is used for converting the position and the speed of the mass center of the head in a lever arm compensation mode to acquire the position and the speed of the head hinge point; and taking the attitude and the attitude angular velocity of the center of mass of the head as the attitude and the attitude angular velocity of the hinged point of the head.
According to an embodiment of the present invention, the third obtaining module 30 obtains a parameter of the car tail hinge point according to a parameter of the car head hinge point, specifically, for obtaining a hinge angle of the car; carrying out difference average filtering on the articulation angle of the vehicle to obtain the articulation angular velocity of the vehicle; acquiring a course angle and a course angular velocity of a vehicle tail hinge point according to the course angle of the vehicle head hinge point, the course angular velocity of the vehicle head hinge point, the hinge angle of the vehicle and the hinge angular velocity of the vehicle; and taking the position and the speed of the hinging point of the head of the bicycle as the position and the speed of the hinging point of the tail of the bicycle.
According to an embodiment of the present invention, the third obtaining module 30 obtains the heading angle and the heading angular velocity of the car tail hinge point according to the heading angle of the car head hinge point, the heading angular velocity of the car head hinge point, the hinge angle of the car and the hinge angular velocity of the car, and is specifically configured to use the sum of the heading angle of the car head hinge point and the hinge angle of the car as the heading angle of the car tail hinge point; and taking the sum of the course angular speed of the vehicle head hinge point and the hinge angle speed of the vehicle as the course angular speed of the vehicle tail hinge point.
According to an embodiment of the present invention, the fourth obtaining module 40 obtains the barycenter parameter of the car tail according to the parameter of the car tail hinge point, and is specifically configured to convert the position and speed of the car tail hinge point by using a lever arm compensation method to obtain the position and speed of the car tail barycenter; and taking the course angle and the course angular speed of the tail hinging point as the course angle and the course angular speed of the tail mass center.
According to one embodiment of the invention, the calculation module 50 calculates the steering command according to the parameters of the centroid of the vehicle tail and the preset reference point parameters, including: calculating errors between the position, the speed, the course angle and the course angular speed of the mass center of the tail of the vehicle and the position, the speed, the course angle and the course angular speed of a preset reference point respectively; and calculating a steering instruction according to the position error, the speed error, the course angle error and the course angular speed error.
According to one embodiment of the invention, the calculation module 50 calculates the position error and the velocity error by the following equations:
wherein the content of the first and second substances,
the error in the position is indicated by a position error,
and
representing the coordinates of the centroid location of the vehicle tail,
and
a position coordinate representing a preset reference point,
which represents the angle of the heading direction,
indicating the course angle of the preset reference point,
the speed error is indicated in the form of a speed error,
representing the east velocity of the center of mass of the vehicle's tail,
representing the east velocity of a preset reference point,
representing the northbound speed of the center of mass of the vehicle tail,
representing the northbound speed of a preset reference point.
According to one embodiment of the invention, the calculation module 50 calculates the steering command by the following formula:
wherein the content of the first and second substances,
a steering command is indicated and the steering is commanded,
、
、
and
the number of the symbols representing the constant number,
the course angular velocity representing the center of mass of the vehicle tail,
indicating the heading angular velocity of a preset reference point.
It should be noted that details that are not disclosed in the control device for articulated vehicle tracking parking according to the embodiment of the present invention refer to details that are disclosed in the control method for articulated vehicle tracking parking according to the embodiment of the present invention, and detailed description thereof is omitted here.
According to the articulated vehicle tracking parking control device provided by the embodiment of the invention, a first acquisition module acquires a mass center parameter of a vehicle head, a second acquisition module acquires a parameter of a vehicle head hinge point according to the mass center parameter of the vehicle head, a third acquisition module acquires a parameter of a vehicle tail hinge point according to the parameter of the vehicle head hinge point, a fourth acquisition module acquires a mass center parameter of a vehicle tail according to the parameter of the vehicle tail hinge point, a calculation module calculates a steering instruction according to the mass center parameter of the vehicle tail and a preset reference point parameter, a control module controls the steering of a vehicle according to the steering instruction, and when the position of the vehicle reaches a parking point, the vehicle is controlled to park. Therefore, smooth parking can be realized, the control error is small, and accurate parking position reaching is realized.
The invention further provides an articulated vehicle corresponding to the embodiment.
Fig. 5 is a block schematic diagram of a vehicle according to an embodiment of the present invention.
As shown in fig. 5, a vehicle 100 of an embodiment of the present invention may include: the above-described control device 110 for tracking and parking an articulated vehicle.
According to the vehicle provided by the embodiment of the invention, the articulated vehicle tracking parking control device can smoothly park the vehicle, has small control error and can accurately reach the parking position.
The invention further provides a computer device corresponding to the embodiment.
The computer device of the embodiment of the invention comprises a memory, a processor and a computer program which is stored on the memory and can run on the processor, and when the processor executes the computer program, the method for controlling the tracking parking of the articulated vehicle according to the embodiment of the invention can be realized.
According to the computer equipment provided by the embodiment of the invention, when the processor executes the computer program stored on the memory, the centroid parameter of the vehicle head is firstly acquired, then the parameter of the vehicle head hinge point is acquired according to the centroid parameter of the vehicle head, then the parameter of the vehicle tail hinge point is acquired according to the parameter of the vehicle head hinge point, the centroid parameter of the vehicle tail is acquired according to the parameter of the vehicle tail hinge point, then the steering instruction is calculated according to the centroid parameter of the vehicle tail and the preset reference point parameter, finally the steering of the vehicle is controlled according to the steering instruction, and when the position of the vehicle reaches the parking point, the vehicle is controlled to park, so that the parking can be carried out smoothly, the control error is small, and.
The invention also provides a non-transitory computer readable storage medium corresponding to the above embodiment.
A non-transitory computer readable storage medium of an embodiment of the present invention stores thereon a computer program, which when executed by a processor, can implement the method for controlling articulated vehicle parking tracking according to the above-described embodiment of the present invention.
According to the non-transitory computer readable storage medium of the embodiment of the invention, when the processor executes the computer program stored on the processor, the centroid parameter of the vehicle head is firstly obtained, then the parameter of the vehicle head hinge point is obtained according to the centroid parameter of the vehicle head, then the parameter of the vehicle tail hinge point is obtained according to the parameter of the vehicle head hinge point, the centroid parameter of the vehicle tail is obtained according to the parameter of the vehicle tail hinge point, then the steering instruction is calculated according to the centroid parameter of the vehicle tail and the preset reference point parameter, finally the steering of the vehicle is controlled according to the steering instruction, and when the position of the vehicle reaches the parking point, the vehicle is controlled to park, so that the vehicle can be parked smoothly, the control error is small, and the accurate.
The present invention also provides a computer program product corresponding to the above embodiments.
The instructions in the computer program product according to the embodiment of the present invention, when executed by the processor, may execute the control method for tracking and parking of the articulated vehicle according to the above-mentioned embodiment of the present invention.
According to the computer program product provided by the embodiment of the invention, when the processor executes the instruction, the mass center parameter of the vehicle head is firstly acquired, then the parameter of the vehicle head hinge point is acquired according to the mass center parameter of the vehicle head, the parameter of the vehicle tail hinge point is acquired according to the parameter of the vehicle head hinge point, the mass center parameter of the vehicle tail is acquired according to the parameter of the vehicle tail hinge point, then the steering instruction is calculated according to the mass center parameter of the vehicle tail and the preset reference point parameter, finally the steering of the vehicle is controlled according to the steering instruction, and when the position of the vehicle reaches the parking point, the vehicle is controlled to park, so that the parking can be smooth, the control error is small, and.
In the description of the present invention, the terms "first" and "second" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implying any number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. The meaning of "plurality" is two or more unless specifically limited otherwise.
In the description herein, references to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, the schematic representations of the terms used above are not necessarily intended to refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.
Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more executable instructions for implementing specific logical functions or steps of the process, and alternate implementations are included within the scope of the preferred embodiment of the present invention in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order, depending on the functionality involved, as would be understood by those reasonably skilled in the art of the present invention.
The logic and/or steps represented in the flowcharts or otherwise described herein, e.g., an ordered listing of executable instructions that can be considered to implement logical functions, can be embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor-containing system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions. For the purposes of this description, a "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of the computer-readable medium would include the following: an electrical connection (electronic device) having one or more wires, a portable computer diskette (magnetic device), a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). Additionally, the computer-readable medium could even be paper or another suitable medium upon which the program is printed, as the program can be electronically captured, via for instance optical scanning of the paper or other medium, then compiled, interpreted or otherwise processed in a suitable manner if necessary, and then stored in a computer memory.
It should be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the various steps or methods may be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any one or combination of the following techniques, which are known in the art, may be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application specific integrated circuit having an appropriate combinational logic gate circuit, a Programmable Gate Array (PGA), a Field Programmable Gate Array (FPGA), or the like.
It will be understood by those skilled in the art that all or part of the steps carried by the method for implementing the above embodiments may be implemented by hardware that is related to instructions of a program, and the program may be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.
In addition, functional units in the embodiments of the present invention may be integrated into one processing module, or each unit may exist alone physically, or two or more units are integrated into one module. The integrated module can be realized in a hardware mode, and can also be realized in a software functional module mode. The integrated module, if implemented in the form of a software functional module and sold or used as a separate product, may also be stored in a computer readable storage medium.
The storage medium mentioned above may be a read-only memory, a magnetic or optical disk, etc. Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention, and that variations, modifications, substitutions and alterations can be made to the above embodiments by those of ordinary skill in the art within the scope of the present invention.
Claims (10)
1. A method for controlling tracked parking of an articulated vehicle, the method comprising the steps of:
acquiring a mass center parameter of the vehicle head;
acquiring parameters of a head hinge point according to the mass center parameters of the head;
acquiring parameters of a vehicle tail hinge point according to the parameters of the vehicle head hinge point;
acquiring a barycenter parameter of the tail of the vehicle according to the parameters of the tail hinging point;
calculating a steering instruction according to the barycenter parameter of the tail of the vehicle and a preset reference point parameter;
and controlling the steering of the vehicle according to the steering command, and controlling the vehicle to stop when the vehicle position reaches a stopping point.
2. The method for controlling the articulated vehicle parking along with the track as claimed in claim 1, wherein the centroid parameters of the head, the head hinge point, the tail hinge point and the tail respectively comprise: position, velocity, attitude, and attitude angular velocity, wherein the attitude comprises: a heading angle, the attitude angular velocity comprising: course angular velocity, according to the barycenter parameter of locomotive obtain the parameter of locomotive pin joint, include:
converting the position and the speed of the mass center of the vehicle head by adopting a lever arm compensation mode to obtain the position and the speed of a hinge point of the vehicle head;
and taking the attitude and the attitude angular velocity of the center of mass of the headstock as the attitude and the attitude angular velocity of the hinging point of the headstock.
3. The method for controlling the articulated vehicle to track and park according to claim 2, wherein the step of obtaining the parameters of the car tail articulated point according to the parameters of the car head articulated point comprises the following steps:
acquiring an articulation angle of the vehicle;
carrying out difference average filtering on the articulation angle of the vehicle to obtain the articulation angular velocity of the vehicle;
acquiring a course angle and a course angular velocity of the vehicle tail hinge point according to the course angle of the vehicle head hinge point, the course angular velocity of the vehicle head hinge point, the hinge angle of the vehicle and the hinge angular velocity of the vehicle;
and taking the position and the speed of the head hinge point as the position and the speed of the tail hinge point.
4. The method for controlling the articulated vehicle to park along the track as claimed in claim 3, wherein the step of obtaining the heading angle and the heading angular velocity of the articulation point at the tail of the vehicle according to the heading angle of the articulation point at the head of the vehicle, the heading angular velocity of the articulation point at the head of the vehicle, the articulation angle of the vehicle and the articulation angular velocity of the vehicle comprises the steps of:
taking the sum of the course angle of the vehicle head hinge point and the hinge angle of the vehicle as the course angle of the vehicle tail hinge point;
and taking the sum of the course angular speed of the vehicle head hinge joint and the hinge angle speed of the vehicle as the course angular speed of the vehicle tail hinge joint.
5. The method for controlling the articulated vehicle to track and park according to claim 4, wherein the step of obtaining the mass center parameter of the tail of the vehicle according to the parameters of the tail-articulating point comprises the following steps:
converting the position and the speed of the tail hinging point by adopting a lever arm compensation mode to obtain the position and the speed of the tail mass center;
and taking the course angle and the course angular speed of the tail hinging point as the course angle and the course angular speed of the tail mass center.
6. The method for controlling the articulated vehicle parking according to the tracking according to claim 5, wherein calculating the steering command according to the centroid parameter of the vehicle tail and the preset reference point parameter comprises:
calculating errors between the position, the speed, the course angle and the course angular speed of the mass center of the vehicle tail and the position, the speed, the course angle and the course angular speed of the preset reference point respectively;
and calculating the steering instruction according to the position error, the speed error, the course angle error and the course angular speed error.
7. The method for controlling articulated vehicle parking tracking according to claim 6, wherein the position error and the speed error are calculated by the following formulas:
wherein the content of the first and second substances,
is indicative of the position error in question,
and
representing the centroid position coordinates of the vehicle tail,
and
position coordinates representing the preset reference point,
a heading angle representing the centroid of the vehicle tail,
indicating a heading angle of the preset reference point,
is indicative of the speed error in question,
representing an east velocity of the center of mass of the vehicle tail,
representing an east velocity of the preset reference point,
representing the northbound speed of the center of mass of the vehicle tail,
representing the north speed of the preset reference point.
8. The method for controlling articulated vehicle parking tracking according to claim 7, wherein the steering command is calculated by the following formula:
wherein the content of the first and second substances,
a direction of the steering command is indicated,
、
、
and
the number of the symbols representing the constant number,
a heading angular velocity representing the center of mass of the vehicle's tail,
indicating the heading angular velocity of a preset reference point.
9. A control device for tracking and parking an articulated vehicle, comprising:
the first acquisition module is used for acquiring a centroid parameter of the locomotive;
the second acquisition module is used for acquiring parameters of a head hinge point according to the mass center parameters of the head;
the third acquisition module is used for acquiring parameters of the car tail hinge point according to the parameters of the car head hinge point;
the fourth acquisition module is used for acquiring the barycenter parameter of the tail of the vehicle according to the parameters of the tail hinging point;
the calculation module is used for calculating a steering instruction according to the barycenter parameter of the vehicle tail and a preset reference point parameter;
and the control module is used for controlling the steering of the vehicle according to the steering instruction and controlling the vehicle to stop when the position of the vehicle reaches a stopping point.
10. An articulated vehicle comprising a control device for tracking parking of an articulated vehicle according to claim 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011351378.8A CN112172800B (en) | 2020-11-27 | 2020-11-27 | Articulated vehicle and tracking parking control method and device thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011351378.8A CN112172800B (en) | 2020-11-27 | 2020-11-27 | Articulated vehicle and tracking parking control method and device thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112172800A CN112172800A (en) | 2021-01-05 |
| CN112172800B true CN112172800B (en) | 2021-03-02 |
Family
ID=73918690
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011351378.8A Active CN112172800B (en) | 2020-11-27 | 2020-11-27 | Articulated vehicle and tracking parking control method and device thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112172800B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113619679B (en) * | 2021-08-27 | 2023-06-20 | 中汽创智科技有限公司 | Reversing tracking method and device for electric automobile, storage medium and terminal |
| CN115325992B (en) * | 2022-10-10 | 2023-12-05 | 南京华擎交通科技有限公司 | Method for measuring automobile train hinge angle based on recursive average filtering |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1795121A (en) * | 2004-02-25 | 2006-06-28 | 戴姆勒-克莱斯勒股份公司 | Control system for a tractor-trailer |
| DE102008004159A1 (en) * | 2008-01-14 | 2009-08-20 | Volkswagen Ag | Driver aiding method for use during maneuvering towing vehicle of e.g. trailer, involves adjusting supporting steering angle at rear axle of towing vehicle depending on predetermined steering angle of steerable wheel of towing vehicle |
| DE102011101990B3 (en) * | 2011-05-19 | 2012-10-18 | Volkswagen Aktiengesellschaft | Method for determining relative drawbar angle in articulated train, involves coupling tensile unit and trailer by rotatably mounted shaft and two-dimensional image is captured by unit for imaging two-dimensional image |
| EP2634071A1 (en) * | 2012-03-03 | 2013-09-04 | WABCO GmbH | Method and device for controlling the parking procedure of an articulated vehicle |
| CN103608242A (en) * | 2011-05-06 | 2014-02-26 | 沃尔沃建筑设备公司 | A method for controlling a working machine, and a working machine |
| CN103963782A (en) * | 2014-05-09 | 2014-08-06 | 济南大学 | Pull type mobile robot parallel parking method |
| CN105264451A (en) * | 2013-05-15 | 2016-01-20 | 沃尔沃卡车集团 | Method for assisting the reversal of an articulated vehicle |
| CN109747637A (en) * | 2019-03-13 | 2019-05-14 | 合肥工业大学 | A kind of semi-trailer train is moved backward into the Induction Control method of position |
| CN111071338A (en) * | 2019-12-30 | 2020-04-28 | 合肥工业大学 | Method for determining articulation angle of linear stable reversing of semi-trailer train |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102007011180A1 (en) * | 2007-03-06 | 2008-09-11 | Daimler Ag | Rangierhilfe and method for drivers of vehicles or vehicle combinations, which consist of mutually bendable vehicle elements |
-
2020
- 2020-11-27 CN CN202011351378.8A patent/CN112172800B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1795121A (en) * | 2004-02-25 | 2006-06-28 | 戴姆勒-克莱斯勒股份公司 | Control system for a tractor-trailer |
| DE102008004159A1 (en) * | 2008-01-14 | 2009-08-20 | Volkswagen Ag | Driver aiding method for use during maneuvering towing vehicle of e.g. trailer, involves adjusting supporting steering angle at rear axle of towing vehicle depending on predetermined steering angle of steerable wheel of towing vehicle |
| CN103608242A (en) * | 2011-05-06 | 2014-02-26 | 沃尔沃建筑设备公司 | A method for controlling a working machine, and a working machine |
| DE102011101990B3 (en) * | 2011-05-19 | 2012-10-18 | Volkswagen Aktiengesellschaft | Method for determining relative drawbar angle in articulated train, involves coupling tensile unit and trailer by rotatably mounted shaft and two-dimensional image is captured by unit for imaging two-dimensional image |
| EP2634071A1 (en) * | 2012-03-03 | 2013-09-04 | WABCO GmbH | Method and device for controlling the parking procedure of an articulated vehicle |
| CN105264451A (en) * | 2013-05-15 | 2016-01-20 | 沃尔沃卡车集团 | Method for assisting the reversal of an articulated vehicle |
| CN103963782A (en) * | 2014-05-09 | 2014-08-06 | 济南大学 | Pull type mobile robot parallel parking method |
| CN109747637A (en) * | 2019-03-13 | 2019-05-14 | 合肥工业大学 | A kind of semi-trailer train is moved backward into the Induction Control method of position |
| CN111071338A (en) * | 2019-12-30 | 2020-04-28 | 合肥工业大学 | Method for determining articulation angle of linear stable reversing of semi-trailer train |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112172800A (en) | 2021-01-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112172800B (en) | Articulated vehicle and tracking parking control method and device thereof | |
| JP6700382B2 (en) | Lane departure detection and lane keeping system for autonomous vehicles with lane curb assistance | |
| JP6430087B1 (en) | Route generating apparatus and vehicle control system | |
| JP6689966B2 (en) | Autonomous vehicle direction detection and lane keeping system based on deceleration curb | |
| JP6668375B2 (en) | Steering control method and system for autonomous vehicle using proportional, integral and derivative (PID) controller | |
| JP6890639B2 (en) | Automatic LIDAR calibration based on cross-validation for autonomous driving | |
| CN111238520A (en) | Lane change path planning method and device, electronic equipment and computer readable medium | |
| JP2020066428A (en) | Two-step reference line smoothing method for mimicking human driving behavior for autonomous driving cars | |
| JP2019189214A (en) | Method of drift correction for path planning of automatic driving vehicle | |
| CN108146424A (en) | Automatic parking controlling of path thereof and control system and vehicle | |
| CN108116497A (en) | The definite method and device of point, steering angle is taken aim in advance for the steering of intelligent driving | |
| JP2019137391A (en) | Self position estimation method for autonomous driving vehicle, system, and machine readable medium | |
| JP2618254B2 (en) | Travel route display device | |
| CN109405837A (en) | Object positioning method, application and vehicle | |
| CN111176298A (en) | Unmanned vehicle track recording and tracking method | |
| CN111634331B (en) | Steering control method, device and system for automatic driving vehicle | |
| US20080170074A1 (en) | Method For Generating a Map Depiction For Optimal Perceptibility of Streets to Travel Through | |
| WO2022217209A1 (en) | Lane changing based only on local information | |
| CN113867343A (en) | Robot moving method, device, robot and storage medium | |
| JPH0544603B2 (en) | ||
| CN112572420B (en) | Articulated vehicle parking control method and device and articulated vehicle | |
| JPH06201392A (en) | Method for forming road map data | |
| JP6704488B1 (en) | Coordinate data correction device, moving body tracking device and moving body tracking method using the same | |
| JP2020087439A (en) | Method to determine vehicle load of autonomous driving vehicle using predetermined load calibration tables | |
| JPH0772926A (en) | Dead reckoning position correction method in dead-reckoning navigation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |