New articulated vehicle control method

Abstract

A new articulated vehicle control method. The method comprises: S100: calculating a transverse position deviation and a yaw angle deviation on the basis of a current pose and a reference path; S200: calculating a curvature deviation on the basis of the current pose and the reference path; and S300: constructing a kinematic error model to solve for a control quantity.

Description

A new control method for articulated vehicles Technical Field

[0001] The present invention belongs to the technical field of automatic control, and in particular relates to a new articulated vehicle control method. Background Art

[0002] Autonomous control of vehicles such as agricultural machinery, loading and unloading trucks, and mining transporters requires answering three fundamental questions: Where am I? Where am I going? How am I getting there? The autonomous driving system responsible for steering the vehicle falls under the third question. The first step in solving the "how to get there" problem is to build a control model. For vehicles with articulated steering, the universal "bicycle model" is not applicable due to their unique two-stage articulated structure. Therefore, a unique kinematic control model must be constructed based on motion and geometric constraints.

[0003] Control models for articulated vehicles are generally based on the vehicle's nonholonomic constraint equations and two-segment rigid-body motion constraint equations. These models construct either a "single-center" error model based on the midpoint of the front wheels of an articulated steering vehicle, or a "dual-center" error model based on the midpoints of the front and rear wheels. While the former is simple, the articulated steering structure can cause the front half of the vehicle to follow the curve while the rear half deviates significantly from the planned curve. The latter, on the other hand, can easily lead to redundant model variables, making subsequent solutions difficult.

[0004] Summary of the Invention

[0005] In order to solve the above technical problems, the present invention provides the following technical solutions:

[0006] A new articulated vehicle control method comprises the following steps:

[0007] S100: Calculating lateral position deviation and yaw angle deviation based on the current posture and reference path;

[0008] S200: Calculating curvature deviation based on the current posture and reference path;

[0009] S300: Construct a kinematic error model to solve the control variable.

[0010] Preferably, in step S100,

[0011] Assume that the current position of the vehicle is P0(x i ,y i ,θ i ), the reference path is Γ(x r (d),y r (s)), where s is a parameter, if the point closest to the current point at this moment is P *(x r (s * ),y r (s * ));

[0012] The current pose is P0(x i ,y i ,θ i ):x i -x coordinate value in the world coordinate system, y i -y coordinate value in the world coordinate system, θ i -The inclination angle of the vehicle coordinate system (with the midpoint of the vehicle's front wheels as the origin and the direction of the vehicle's front as the positive X-axis, in accordance with the right-hand rule) in the world coordinate system, that is, the current posture of the vehicle;

[0013] The reference path is Γ(x r (s),y r (s)): x r (s) - the reference path x coordinate expression in the world coordinate system, y r (s) - y-coordinate expression of the reference path in the world coordinate system, s - is the independent variable parameter;

[0014] s * : The independent variable parameter corresponding to the point closest to the current point in the reference path;

[0015] Then P * (x r (s * ),y r (s * The tangent unit vector at )) is

[0016] Then P0(x i ,y i ,θ i The absolute deviation vector at ) is

[0017] Then P0(x i ,y i ,θ i ) at the lateral position deviation ε y for and The vector product of:

[0018] Then P0(x i ,y i ,θ i ) at the yaw angle deviation ε θ :

[0019] Preferably, in step S200,

[0020] P * (x r (s * ),y r (s * ))Path curvature at:

[0021] Based on the distance from the midpoint of the front wheel and the midpoint of the rear wheel to the hinge and the hinge angle, it is easy to find an approximate circle tangent to the two midpoints, whose radius is r.

[0022] Then P0(x i ,y i ,θ i ) at curvature deviation:

[0023] Preferably, in step S300,

[0024] Nonholonomic constraint equations:

[0025] 2-segment rigid body constraint equation:

[0026] (x0, y0) and (x1, y1) are the coordinates of the front wheel midpoint and the rear wheel midpoint in the world coordinate system, respectively. θ0 and θ1 are the inclination angles of the front wheel midpoint coordinate system (with the front wheel midpoint as the origin and the vehicle's forward direction as x-positive, in accordance with the right-hand rule) and the rear wheel midpoint coordinate system (with the rear wheel midpoint as the origin and the vehicle's forward direction as x-positive, in accordance with the right-hand rule) in the world coordinate system, i.e., the current posture of the front and rear parts of the vehicle;

[0027] According to the non-holonomic constraint equation and the two-segment rigid body constraint equation of the articulated vehicle, the error vector Differentiate The relationship between

[0028] Where L0 is the distance from the front axle (the distance from the hinge point to the center of the front wheel), L1 is the distance from the rear axle, and the total wheelbase L = L0 + L1. After a slight manipulation of the state variables, we get:

[0029] v0: velocity of the front part of the vehicle;

[0030] γ: angle at the hinge;

[0031] Angular velocity at the joint;

[0032] Angular acceleration at the joint;

[0033] Solve the equation using standard LQR or MPC to obtain the control quantity

[0034] Compared with the prior art, the beneficial effects of the present invention are: in order to solve the problem of the consistency of the motion trajectory of the front and rear parts of the articulated steering vehicle with the planned curve, it is also not desired to introduce additional constraints in the control model to reduce the difficulty of subsequent solutions. Taking full account of the structural characteristics of the articulated steering vehicle, a new control model is proposed. This control model also takes the midpoint of the front wheel of the vehicle as the reference center, but in the error model, in addition to the lateral position deviation, yaw angle deviation, etc., it also creatively proposes the concept of curvature deviation. This curvature deviation can accurately describe the problem of the accuracy of the motion trajectory of the front and rear parts of the articulated vehicle and the planned curve, so that the control error model of the articulated vehicle can be fully described by fewer state variables, making full use of the advantages of the simplicity of the "single center" and the ability of the "dual center" to solve the problem of the consistency of the motion trajectory of the front and rear parts of the vehicle with the planned curve.

[0035] The working principle of the present invention is as follows: the vehicle body is equipped with a GNSS antenna or other positioning data measurement device, as well as an angle sensor that detects the vehicle's real-time articulation angle, or an inertial measurement unit installed on the vehicle body is used to estimate the real-time angle and angular velocity of the articulation point, and then substitute these into the model formula to solve the problem of the movement trajectory of the front and rear parts of the vehicle being consistent with the path during curved driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0037] FIG1 is a schematic flow diagram of the present invention. DETAILED DESCRIPTION

[0038] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0039] The present invention provides a technical solution, and the specific flow chart is shown in FIG1 . The specific execution process of the method is as follows.

[0040] A GNSS antenna or other positioning data measurement device must be installed on the vehicle body. In addition, an angle sensor must be installed to detect the vehicle's real-time articulation angle, or an inertial measurement unit installed on the vehicle body must be used to estimate the real-time angle and angular velocity of the articulation.

[0041] 1. Calculate the lateral position deviation and yaw angle deviation based on the current posture and reference path:

[0042] Assume that the current position of the vehicle is P0(x i ,y i ,θ i ), the reference path is Γ(x r (s),y r (s)), where s is a parameter. If the point closest to the current point at this moment is P * (x r (s * ),y r (s * ));

[0043] The current pose is P0(x i ,y i ,θ i ):x i -x coordinate value in the world coordinate system, y i -y coordinate value in the world coordinate system, θ i -The inclination angle of the vehicle coordinate system (with the midpoint of the vehicle's front wheels as the origin and the direction of the vehicle's front as the positive X-axis, in accordance with the right-hand rule) in the world coordinate system, that is, the current posture of the vehicle;

[0044] The reference path is Γ(x r (s),y r (s)): x r (s) - the reference path x coordinate expression in the world coordinate system, y r (s) - y-coordinate expression of the reference path in the world coordinate system, s - is the independent variable parameter;

[0045] s * : The independent variable parameter corresponding to the point closest to the current point in the reference path;

[0046] Then P * (x r (s * ),y r (s * The tangent unit vector at )) is

[0047] Then P0(x i ,y i ,θ i The absolute deviation vector at ) is

[0048] Then P0(x i ,y i ,θ i ) at the lateral position deviation ε y for and The vector product of:

[0049] Then P0(x i ,y i ,θ i ) at the yaw angle deviation ε θ :

[0050] 2. Calculate curvature deviation based on current pose and reference path:

[0051] P * (x r (s * ),y r (s * ))Path curvature at:

[0052] Based on the distance from the midpoint of the front wheel and the midpoint of the rear wheel to the hinge and the hinge angle, it is easy to calculate an approximate circle tangent to the two midpoints, whose radius is r.

[0053] Then P0(x i ,y i ,θ i ) at curvature deviation:

[0054] 3. Construct kinematic error model:

[0055] Nonholonomic constraint equations:

[0056] 2-segment rigid body constraint equation:

[0057] (x0, y0) and (x1, y1) are the coordinate values ​​of the front wheel midpoint and the rear wheel midpoint in the world coordinate system respectively, θ0 and θ1 are the inclination angles of the front wheel midpoint coordinate system (with the front wheel midpoint as the origin and the vehicle forward direction as x-positive, in accordance with the right-hand rule) and the rear wheel midpoint coordinate system (with the rear wheel midpoint as the origin and the vehicle forward direction as x-positive, in accordance with the right-hand rule) in the world coordinate system, that is, the current posture of the front and rear parts of the vehicle.

[0058] According to the non-holonomic constraint equation and the two-segment rigid body constraint equation of the articulated vehicle, the error vector Differentiate The relationship between

[0059] Where L0 is the distance from the front axle (the distance from the hinge point to the center of the front wheel), L1 is the distance from the rear axle, and the total wheelbase L = L0 + L1. After a slight manipulation of the state variables, we get:

[0060] v0: velocity of the front part of the vehicle;

[0061] γ: angle at the hinge;

[0062] Angular velocity at the joint;

[0063] Angular acceleration at the joint.

[0064] 4. Solving the model:

[0065] The above model is a typical first-order linear state equation, which can be solved by standard LQR or MPC to obtain the control quantity

[0066] LQR (Linear Quadratic Regulator) and MPC (Model Predictive Control) are two commonly used methods for solving first-order linear state equations. The former is a form of full-state feedback control, while the latter is a form of rolling-edge optimization control. Both aim to find the optimal control solution by minimizing a weighted cost function between the state vector and the input. Both have mature toolboxes available in MATLAB.

[0067] This embodiment is directed to the construction of a control model for an articulated steering vehicle. It is simple and convenient, and can solve the problem of ensuring that the front and rear parts of the vehicle are consistent with the path during curved driving.

[0068] It should be noted that, in this document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that includes a list of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus.

[0069] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art will be able to modify the technical solutions described in the aforementioned embodiments or substitute equivalents for some of the technical features. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A new control method for articulated vehicles, characterized in that, it includes the following steps: S100: Calculate the lateral position deviation and yaw angle deviation based on the current pose and the reference path; S200: Calculate the curvature deviation based on the current pose and the reference path; S300: Construct a kinematic error model to solve for the control quantity.

2. The new control method for articulated vehicles according to claim 1, characterized in that, in the step S100, Assume that the current pose of the vehicle is P 0 (x i , y i , θ i ), and the reference path is Γ(x r (s), y r (s)), where s is a parameter. If the point closest to the current point at this moment is P * (x r (s * ), y r (s * )); The current pose is P 0 (x i , y i , θ i ): x i - The x - coordinate value in the world coordinate system, y i - The y - coordinate value in the world coordinate system, θ i - The inclination angle of the vehicle coordinate system (with the mid - point of the front wheels of the vehicle as the origin and the vehicle head direction as the positive X - axis, conforming to the right - hand rule) in the world coordinate system, that is, the current attitude of the vehicle; The reference path is Γ(x r (s), y r (s)): x r (s) - the x - coordinate expression of the reference path in the world coordinate system, y r (s) - the y - coordinate expression of the reference path in the world coordinate system, s - the independent variable parameter; s * : The independent variable parameter corresponding to the point closest to the current point in the reference path; Then P * (x r (s * ),y r (s * )) the tangent unit vector at is Then P 0 (x i , y i , θ i ) the absolute deviation vector at is Then P 0 (x i , y i , θ i ) the lateral position deviation ε y is With Vector product of: Then P 0 (x i , y i , θ i ) at the yaw angle deviation ε θ :

3. The new control method for articulated vehicles according to claim 1, characterized in that, in the step S200, P * (x r (s * ),y r (s * ) is the path curvature at: Based on the distances from the midpoints of the front wheels and the rear wheels of the vehicle to the articulation point and the articulation angle, it is easy to obtain an approximate circle tangent to the two midpoints, and its radius is r, Then P 0 (x i , y i , θ i ) curvature deviation at:

4. The new control method for articulated vehicles according to claim 1, characterized in that, in the step S300, Non-integrity constraint equation: Two-segment rigid body constraint equation: (x 0 , y 0 ), (x 1 , y 1 ) are the coordinate values of the midpoints of the front wheel and the rear wheel in the world coordinate system respectively, and θ 0 , θ 1 are the inclination angles of the midpoint coordinate system of the front wheel (with the midpoint of the front wheel as the origin, the forward direction of the vehicle as the positive x-axis, conforming to the right-hand rule) and the midpoint coordinate system of the rear wheel (with the midpoint of the rear wheel as the origin, the forward direction of the vehicle as the positive x-axis, conforming to the right-hand rule) in the world coordinate system, that is, the current postures of the front and rear parts of the vehicle; The error vector can be obtained according to the non-integrity constraint equation and the rigid body constraint equation of the two segments of the articulated vehicle and its differential The relationship between Where L 0 is the front axle distance (the distance from the hinge point to the midpoint of the front wheels), and L 1 is the rear axle distance. The total wheelbase L = L 0 + L 1 In order to eliminate Slightly process the state variable to obtain: v 0 : The speed of the front part of the vehicle; γ: the angle at the articulation point; the angular velocity at the articulation point; the angular acceleration at the articulation point; The standard LQR or MPC is used to solve the equation to obtain the control quantity

Images