A navigation control method and related device for an articulated vehicle

Abstract

This application discloses a navigation control method and related apparatus for an articulated vehicle, relating to the field of autonomous driving control. The method includes: acquiring real-time navigation information of the articulated vehicle; calculating the steering curvature error based on the navigation information and a preset discrete error tracking model; calculating the expected value of the steering curvature based on the steering curvature error and a reference value of the steering curvature; calculating the expected value of the hinge angle based on the expected value of the steering curvature; and generating a steering drive command to control the steering motor to drive the hinge angle to change to the expected value of the hinge angle. The discrete error tracking model describes the linear relationship between the discrete pose tracking error vector and the steering curvature error. The pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. This method introduces an intermediate quantity, steering curvature, based on the motion characteristics of the articulated vehicle, linearizing the motion state model of the articulated vehicle and improving the control accuracy of the articulated vehicle's curve tracking.

Description

Technical Field

[0001] This application relates to the field of autonomous driving control technology, and in particular to a navigation control method and related device for an articulated vehicle. Background Technology

[0002] With the development of MESMS (Micro-Electro-Mechanical-System) sensor, navigation, and control technologies, precision agriculture is rapidly becoming a trend. Existing agricultural machinery navigation systems are primarily developed for front-wheel steering vehicles. When these systems are applied to articulated vehicles, the navigation control methods are poorly adapted, resulting in low accuracy in the navigation control of articulated vehicles. Summary of the Invention

[0003] In view of the above problems, this application provides a navigation control method and related device for articulated vehicles to improve the navigation control accuracy of articulated vehicles. The specific solution is as follows:

[0004] The first aspect of this application provides a navigation control method for an articulated vehicle, comprising:

[0005] The navigation information of the articulated vehicle is acquired in real time. The navigation information includes real-time pose information and speed. The real-time pose information includes the real-time values ​​of the X-axis coordinate, Y-axis coordinate, and heading angle of the articulated vehicle in the global coordinate system.

[0006] Based on the navigation information of the articulated vehicle and the preset discrete error tracking model, the steering curvature error is calculated;

[0007] The discrete error tracking model is used to describe the linear relationship between the discrete pose tracking error vector and the steering curvature error. The pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinates of the articulated vehicle and the target reference point. The Y-axis tracking error is the difference between the Y-axis coordinates of the articulated vehicle and the target reference point. The heading angle tracking error is the difference between the heading angle of the articulated vehicle and the heading angle of the target reference point. The steering curvature error is the difference between the expected value of the steering curvature of the articulated vehicle and the steering curvature of the target reference point.

[0008] Based on the steering curvature error and the steering curvature of the target reference point, the expected value of the steering curvature is calculated;

[0009] Based on the expected value of the steering curvature, calculate the expected value of the hinge angle;

[0010] Generate steering drive commands to control the steering motor to change the hinge angle to the desired value of the hinge angle.

[0011] In one possible implementation, the navigation control method for the articulated vehicle also includes:

[0012] A motion state model of the articulated vehicle is constructed; the kinematic model of the articulated vehicle is used to describe the relationship between the pose vector and the steering curvature, wherein the pose vector is composed of the X-axis coordinate value, Y-axis coordinate value and heading angle of the articulated vehicle in the global coordinate system;

[0013] The motion state model is linearized to construct a continuous error tracking model for the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the pose tracking error vector and the steering curvature error in a continuous form.

[0014] Based on a preset sampling time, the continuous error tracking model is discretized to obtain the discrete error tracking model.

[0015] In one possible implementation, the steering curvature error is calculated based on the articulated vehicle's navigation information and a preset discrete error tracking model, including:

[0016] Based on the preset discrete error tracking model, the performance function of the linear quadratic regulator LQR is constructed.

[0017] Based on the navigation information of the articulated vehicle and the Ricardi equation, the LQR performance function is solved to obtain the optimal value of the steering curvature error.

[0018] In one possible implementation, the expected value of the steering curvature error is calculated based on the steering curvature error and the steering curvature of the target reference point, including:

[0019] Based on the real-time pose information of the articulated vehicle, the target reference point is determined from the preset reference path, and the pose information of the target reference point is obtained. The pose information of the target reference point includes the X-axis coordinate value, Y-axis coordinate value, and heading angle of the target reference point in the global coordinate system.

[0020] The distances between reference points on the reference path located after the target reference point and the target reference point are calculated sequentially from near to far until the distance is greater than a preset aiming distance threshold, thus obtaining the aiming reference point and the aiming distance.

[0021] Based on the pose information of the pre-aiming reference point and the target reference point, the turning curvature of the target reference point on the reference path is calculated and used as a reference value for the turning curvature;

[0022] The sum of the optimal value of the steering curvature error and the reference value of the steering curvature is calculated as the expected value of the steering curvature.

[0023] In one possible implementation, calculating the expected value of the hinge angle based on the expected value of the steering curvature includes:

[0024] Based on the preset articulated vehicle steering model and the expected value of the steering curvature, the gradient descent algorithm is used to calculate the expected value of the articulation angle. The articulated vehicle steering model is used to describe the relationship between the articulation angle and the steering curvature.

[0025] A second aspect of this application provides a navigation control device for an articulated vehicle, comprising:

[0026] The navigation monitoring unit is used to acquire the navigation information of the articulated vehicle in real time. The navigation information includes real-time pose information and speed. The real-time pose information includes the real-time values ​​of the X-axis coordinate, Y-axis coordinate, and heading angle of the articulated vehicle in the global coordinate system.

[0027] The curvature error monitoring unit is used to calculate the steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model.

[0028] The discrete error tracking model is used to describe the linear relationship between the discrete pose tracking error vector and the steering curvature error. The pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinates of the articulated vehicle and the target reference point. The Y-axis tracking error is the difference between the Y-axis coordinates of the articulated vehicle and the target reference point. The heading angle tracking error is the difference between the heading angle of the articulated vehicle and the heading angle of the target reference point. The steering curvature error is the difference between the steering curvature of the articulated vehicle and the steering curvature of the target reference point.

[0029] The curvature prediction unit is used to calculate the expected value of the steering curvature based on the steering curvature error and the reference value of the steering curvature.

[0030] The hinge angle prediction unit is used to calculate the expected value of the hinge angle based on the expected value of the steering curvature.

[0031] A navigation control unit is used to generate steering drive commands to control the steering motor to drive the hinge angle to change to the desired value of the hinge angle.

[0032] In one possible implementation, the navigation control unit of the articulated vehicle further includes: a model building unit, used for:

[0033] A motion state model of the articulated vehicle is constructed; the kinematic model of the articulated vehicle is used to describe the relationship between the pose vector and the steering curvature, wherein the pose vector is composed of the X-axis coordinate value, Y-axis coordinate value and heading angle of the articulated vehicle in the global coordinate system;

[0034] The motion state model is linearized to construct a continuous error tracking model for the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the pose tracking error vector and the steering curvature error in a continuous form.

[0035] Based on a preset sampling time, the continuous error tracking model is discretized to obtain the discrete error tracking model.

[0036] A third aspect of this application provides a computer program product including computer-readable instructions that, when executed on an electronic device, cause the electronic device to implement the navigation control method for an articulated vehicle as described in the first aspect or any implementation thereof.

[0037] A fourth aspect of this application provides an electronic device, including at least one processor and a memory connected to the processor, wherein:

[0038] The memory is used to store computer programs;

[0039] The processor is used to execute the computer program so that the electronic device can implement the navigation control method for the articulated vehicle described in the first aspect or any implementation thereof.

[0040] The fifth aspect of this application provides a computer storage medium carrying one or more computer programs that, when executed by an electronic device, enable the electronic device to implement the navigation control method for an articulated vehicle as described in the first aspect or any implementation thereof.

[0041] By employing the above technical solution, this application provides a navigation control method and related device for an articulated vehicle. This method acquires the articulated vehicle's navigation information in real time, calculates the steering curvature error based on the navigation information and a preset discrete error tracking model, calculates the expected value of the steering curvature based on the steering curvature error and a reference value of the steering curvature, and calculates the expected value of the articulation angle based on the expected value of the steering curvature. A steering drive command is generated to control the steering motor to drive the articulation angle to change to the expected value of the articulation angle. The navigation information includes real-time pose information and velocity. The real-time pose information includes the real-time values ​​of the articulated vehicle's X-axis coordinates, Y-axis coordinates, and heading angle in the global coordinate system. Since the discrete error tracking model is used to describe the linear relationship between the discrete pose tracking error vector and the steering curvature error, the pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinates of the articulated vehicle and the target reference point; the Y-axis tracking error is the difference between the Y-axis coordinates of the articulated vehicle and the target reference point; the heading angle tracking error is the difference between the heading angles of the articulated vehicle and the target reference point; and the steering curvature error is the difference between the steering curvature of the articulated vehicle and the steering curvature of the target reference point. In other words, by introducing the intermediate quantity of steering curvature based on the motion characteristics of the articulated vehicle, the motion state model of the articulated vehicle is linearized, overcoming the computational difficulty caused by the nonlinear characteristics of the articulated vehicle's position and articulation angle, and thus improving the control accuracy of the articulated vehicle's curve tracking. Attached Figure Description

[0042] The above and other features, advantages, and aspects of the embodiments of this disclosure will become more apparent from the accompanying drawings and the following detailed description. Throughout the drawings, the same or similar reference numerals denote the same or similar elements. It should be understood that the drawings are schematic, and the originals and elements are not necessarily drawn to scale.

[0043] Figure 1 A steering diagram of an articulated vehicle provided in an embodiment of this application;

[0044] Figure 2 A flowchart illustrating a navigation control method for an articulated vehicle provided in an embodiment of this application;

[0045] Figure 3 A flowchart illustrating the specific implementation of the navigation control method for the articulated vehicle provided in this application embodiment;

[0046] Figure 4 A schematic diagram of the structure of a navigation control device for an articulated vehicle provided in an embodiment of this application;

[0047] Figure 5 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0048] The embodiments of this application are described below with reference to the accompanying drawings. The terminology used in the implementation section of this application is for explaining specific embodiments only and is not intended to limit the scope of this application.

[0049] The embodiments of this application will now be described with reference to the accompanying drawings. Those skilled in the art will recognize that, with technological advancements and the emergence of new scenarios, the technical solutions provided in the embodiments of this application are equally applicable to similar technical problems.

[0050] The terms "first," "second," etc., used in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such terms are interchangeable where appropriate; this is merely a way of distinguishing objects with the same attributes in the embodiments of this application. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion, so that a process, method, system, product, or apparatus that comprises a series of elements is not necessarily limited to those elements but may include other elements not explicitly listed or inherent to those processes, methods, products, or apparatuses.

[0051] Navigation control of articulated vehicles (ARTVs) requires real-time steering control, typically achieved by controlling the articulation angle to steer the vehicle and thus enable it to drive automatically along a reference path. However, due to the different motion characteristics of various vehicle types, and the non-linear relationship between the articulation angle (a navigation control parameter) and the position of the articulated vehicle, existing navigation control systems designed for other vehicle types have poor adaptability when applied to articulated vehicles.

[0052] To address nonlinear characteristics, existing technologies determine the optimal function of nonlinear model predictive control based on a planned segmented reference path, aiming to optimize the articulated vehicle's tracking performance and the smoothness of control variable outputs. This yields the optimal control sequence for the articulated vehicle to track the planned segmented reference path. However, this existing technology involves significant computational demands, requires sophisticated hardware platforms, and often results in low control accuracy.

[0053] In particular, during actual autonomous driving, there are complex control requirements such as irregular terrain, obstacle avoidance, and U-turns, which require tracking curved paths to perform autonomous driving tasks. Therefore, how to improve the curve trajectory tracking control accuracy of articulated vehicles is a technical problem that urgently needs to be solved by those skilled in the art.

[0054] Figure 1 This application provides a schematic diagram of the steering of an articulated vehicle, as shown in the embodiment of the present application. Figure 1As shown, the articulated vehicle is composed of a tractor 101 and a trailer 102 connected by an articulation mechanism. Here, r is the turning radius of the articulated vehicle, L1 is the distance from the articulation point to the center of the rear axle, L2 is the distance from the articulation point to the center of the front axle, and the articulation angle is... The articulation angle refers to the angle between the tractor and the trailer, which is the difference between the tractor's heading angle and the trailer's heading angle. The size of the articulation angle directly reflects the relative positional relationship between the tractor and the trailer.

[0055] This application provides a navigation control method for an articulated vehicle. This method can be applied to the field of autonomous driving control, specifically to an articulated vehicle autonomous driving scenario. It tracks the reference path of the articulated vehicle and, based on the current pose information of the articulated vehicle, controls the navigation of the articulated vehicle by controlling the articulation angle, so that the articulated vehicle can drive autonomously along the reference path.

[0056] It should be noted that this method can be applied to the vehicle controller of an articulated vehicle, where a navigation control system is configured, as shown in the reference. Figure 2 , Figure 2 A flowchart illustrating a navigation control method for an articulated vehicle provided in this application embodiment is shown below. Figure 2 As shown, this method may include steps S201 to S205, which are described in detail below.

[0057] S201. Obtain navigation information of the articulated vehicle in real time.

[0058] In this embodiment, the navigation information includes real-time pose information and speed. The real-time pose information includes the real-time values ​​of the X-axis coordinates, Y-axis coordinates, and heading angle of the articulated vehicle in the global coordinate system.

[0059] In this embodiment, the high-precision navigation and positioning system installed on the articulated vehicle has a built-in high-precision positioning and orientation board, which can quickly and accurately calculate the relative position information and azimuth angle of the two antennas. At the same time, by receiving differential data from the base station, real-time carrier phase differential positioning can be achieved, thereby obtaining navigation information at the centimeter level.

[0060] It should be noted that the specific method for constructing a global coordinate system is described in existing technologies.

[0061] S202. Based on the navigation information of the articulated vehicle and the preset discrete error tracking model, calculate the steering curvature error.

[0062] The discrete error tracking model describes the linear relationship between the discrete pose tracking error vector and the steering curvature error. The pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinates of the articulated vehicle and the target reference point. The Y-axis tracking error is the difference between the Y-axis coordinates of the articulated vehicle and the target reference point. The heading angle tracking error is the difference between the heading angle of the articulated vehicle and the heading angle of the target reference point. The steering curvature error is the difference between the steering curvature of the articulated vehicle and the steering curvature of the target reference point.

[0063] In one optional embodiment, the state equation expression of the discrete error tracking model is: ,in: Representing time in discrete form, for The state variable at time t, that is, The pose tracking error vector at time step (k). for The state variable at time t, that is The pose tracking error vector at time step (k). for The state transition matrix of the error tracking model at time step 1, that is, the discrete form of the state transition matrix. for The control variable at time, that is The turning curvature error at any given time. for The discrete form of the control matrix of the discrete error tracking model at time T is the discrete time step, i.e., the sampling time interval.

[0064] Specifically, the discrete forms of the state transition matrix and the control matrix are as follows: .

[0065] In one optional embodiment, a discrete error tracking model is obtained by linearizing and discretizing the motion state model of the articulated vehicle by introducing steering curvature. It is understood that the state variables and control variables in the discrete error tracking model have a linear relationship. The navigation information of the articulated vehicle is substituted into the discrete error tracking model, and the optimal value of the control variables under this navigation information is calculated as the steering curvature error.

[0066] S203. Based on the steering curvature error and the reference value of steering curvature, calculate the expected value of steering curvature.

[0067] In this embodiment, the reference value of the steering curvature is the steering curvature of the target reference point on the reference trajectory. The steering curvature error represents the difference between the steering curvature of the articulated vehicle and the steering curvature of the target reference point. Therefore, the steering curvature error represents the difference between the expected value of the steering curvature and the steering curvature of the target reference point on the reference trajectory. Thus, the expected value of the steering curvature can be obtained by adding the steering curvature error and the reference value of the steering curvature.

[0068] S204. Calculate the expected value of the hinge angle based on the expected value of the steering curvature.

[0069] In this embodiment, based on the geometric characteristics of the articulated vehicle during movement, the relationship between the steering curvature and the articulation angle is determined. This relationship allows for the calculation of the expected value of the articulation angle under the expected value of the steering curvature.

[0070] S205. Generate steering drive command to control the steering motor to drive the hinge angle change to the desired hinge angle value.

[0071] In this embodiment, the drive command is used to control the steering motor to change the hinge angle to the desired value of the hinge angle. The command interaction method between the navigation system and the steering motor is described in the prior art.

[0072] As can be seen from the above technical solution, the navigation control method for an articulated vehicle provided in this application acquires the navigation information of the articulated vehicle in real time. Based on the navigation information and a preset discrete error tracking model, it calculates the steering curvature error. Based on the steering curvature error and a reference value of the steering curvature, it calculates the expected value of the steering curvature. Based on the expected value of the steering curvature, it calculates the expected value of the hinge angle. It generates a steering drive command to control the steering motor to drive the hinge angle to change to the expected value of the hinge angle. Since the discrete error tracking model is used to describe the linear relationship between the discrete pose tracking error vector and the steering curvature error, the pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinates of the articulated vehicle and the target reference point; the Y-axis tracking error is the difference between the Y-axis coordinates of the articulated vehicle and the target reference point; the heading angle tracking error is the difference between the heading angles of the articulated vehicle and the target reference point; and the steering curvature error is the difference between the steering curvature of the articulated vehicle and the steering curvature of the target reference point. In other words, by introducing the intermediate quantity of steering curvature based on the motion characteristics of the articulated vehicle, the motion state model of the articulated vehicle is linearized, overcoming the computational difficulty caused by the nonlinear characteristics of the articulated vehicle's position and articulation angle, and thus improving the control accuracy of the articulated vehicle's curve tracking.

[0073] Furthermore, embodiments of this application provide a specific implementation method for the navigation control of an articulated vehicle. Figure 3A flowchart illustrating the specific implementation of the navigation control method for the articulated vehicle provided in this application embodiment is shown below. Figure 3 As shown, this method specifically includes:

[0074] S301. Construct a motion state model for the articulated vehicle.

[0075] In this embodiment, the state variables of the kinematic model of the articulated vehicle are the X-axis coordinates of the articulated vehicle in the global coordinate system. Y-axis coordinate value and heading angle The resulting pose vector is denoted as The control variable for the kinematic model of the articulated car is the steering curvature k, denoted as... The kinematic model of an articulated vehicle is used to describe state variables. and control variables The mathematical relationship between them.

[0076] In this embodiment, the state equation of the motion state model of the articulated vehicle is shown in equation (1):

[0077] (1).

[0078] in, This represents the linear velocity of the articulated vehicle along the X-axis in the global coordinate system. The linear velocity of the articulated vehicle along the Y-axis in the global coordinate system. This indicates the angular velocity of the articulated vehicle's heading. This indicates the speed of the articulated vehicle. , , These represent the X-axis coordinates, Y-axis coordinates, and the rate of change of the heading angle along the driving path, respectively.

[0079] S302. Linearize the motion state model to obtain the continuous error tracking model of the articulated vehicle.

[0080] In this embodiment, the state equation of the motion state model is Taylor expanded at the reference point, and higher-order terms are ignored to obtain the continuous error tracking model of the articulated vehicle.

[0081] In this embodiment, the state variable in the continuous error tracking model is the pose tracking error vector, and the control variable is the steering curvature error.

[0082] The pose tracking error vector consists of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error. The X-axis tracking error is the X-axis coordinate value of the actual position of the articulated vehicle in the global coordinate system. X-axis coordinates relative to the target reference point The difference, the Y-axis tracking error is the Y-axis coordinate value of the articulated vehicle in the global coordinate system. Y-axis coordinates relative to the target reference point The difference, the heading angle tracking error is the heading angle of the articulated vehicle in the global coordinate system. Heading angle relative to the target reference point The difference, the steering curvature error is the steering curvature of the articulated vehicle in the global coordinate system. The turning curvature of the target reference point The difference. The pose tracking error vector is denoted as... The control variable is denoted as .

[0083] In this embodiment, the error tracking model is used to describe the state variables. and control variables The mathematical relationship between them is shown in equation (2) of the state equation of the error tracking model:

[0084] (2)

[0085] It should be noted that this step linearizes the motion state model by constructing an error tracking model.

[0086] S303. Based on the preset sampling time, the continuous error tracking model is discretized to obtain a discrete error tracking model.

[0087] In this embodiment, the continuous error tracking model is discretized using equation (3), and the state equation expression of the discrete error tracking model is shown in equation (3):

[0088] (3)

[0089] in: Representing time in discrete form, for The state variable at time t, that is, The pose tracking error vector at time step (k). for The state variable at time t, that is The pose tracking error vector at time step (k). for The state transition matrix of the error tracking model at time step 1, that is, the discrete form of the state transition matrix. for The control variable at time, that is The turning curvature error at any given time. for The discrete form of the control matrix of the discrete error tracking model at time T is the discrete time step, i.e., the sampling time interval.

[0090] Specifically, the discrete forms of the state transition matrix and the control matrix are shown in equation (4):

[0091] (4).

[0092] It should be noted that after constructing the motion state model of the articulated vehicle through S301~S303, this solution linearizes the motion state model to obtain a continuous error tracking model. Furthermore, the continuous error tracking model is discretized to obtain a discrete error tracking model, which is then deployed in the vehicle controller.

[0093] S304. Obtain navigation information for the articulated vehicle in real time.

[0094] In this embodiment, the navigation information includes real-time pose information and real-time velocity, wherein the real-time pose information includes the real-time value of the X-axis coordinate in the global coordinate system. Real-time values ​​of Y-axis coordinates Real-time values ​​of heading angle .

[0095] In this embodiment, the high-precision navigation and positioning system installed on the articulated vehicle has a built-in high-precision positioning and orientation board, which can quickly and accurately calculate the relative position information and azimuth angle of the two antennas. At the same time, by receiving differential data from the base station, real-time carrier phase differential positioning can be achieved, thereby obtaining navigation information at the centimeter level.

[0096] S305. Based on the real-time pose information of the articulated vehicle, determine the target reference point from the reference path and obtain the pose information of the target reference point.

[0097] In this embodiment, the reference path is a curved path composed of multiple reference points, and the target reference point is the reference point on the reference path that is closest to the current position of the articulated vehicle.

[0098] In this embodiment, the target reference point is denoted as... ( , , ),in, This represents the X-axis coordinate value of the target reference point. This represents the Y-axis coordinate value of the target reference point. The heading angle represents the target reference point.

[0099] S306. Calculate the distance between the reference point located after the target reference point on the reference path and the target reference point in order from near to far, until the distance is greater than the preset aiming distance threshold, and obtain the aiming reference point and aiming distance.

[0100] In this embodiment, the distance between the pre-aiming reference point and the target reference point is greater than a preset pre-aiming distance threshold, and the distance between the previous reference point of the pre-aiming reference point and the target reference point is less than the pre-aiming distance threshold. The distance between the pre-aiming reference point and the target reference point is used as the pre-aiming distance.

[0101] In this embodiment, formula (5) is used to calculate the i-th reference point after the target reference point. ( , , Distance from the target reference point :

[0102] (5)

[0103] Let the aiming reference point be the nth reference point after the target reference point. That is, the aiming distance > .

[0104] S307. Calculate the turning curvature of the target reference point on the reference path based on the pose information of the pre-aiming reference point and the target reference point, and use it as a reference value for the turning curvature.

[0105] In this embodiment, the turning curvature of the target reference point on the reference path is calculated using formula (6).

[0106] (6).

[0107] S308. Based on the discrete error tracking model, construct the LQR performance function.

[0108] In this embodiment, the performance function of LQR (Linear Quadratic Regulator) is as shown in formula (7):

[0109] (7).

[0110] in, It is a positive semidefinite symmetric weighted matrix. It is a positive semidefinite symmetric weighted matrix. and These are the start and end times, respectively. Formula (7) is a quadratic function, representing a comprehensive performance index of control deviation and control smoothness; the performance is best when the solution is minimized. The LQR control parameter is the steering curvature error. .

[0111] S308. Based on the navigation information of the articulated vehicle and the Riccati equation, solve the LQR performance function to obtain the optimal value of the steering curvature error. In this embodiment, the Riccati equation is as shown in formula (8):

[0112] (8)

[0113] Calculate the LQR control parameters using formula (8) The optimal value of the steering curvature error is obtained:

[0114] (9)

[0115] S309. Based on the optimal value of the steering curvature error and the reference value of the steering curvature, calculate the expected value of the steering curvature.

[0116] The expected value of the turning curvature is calculated using formula (10). :

[0117] (10)

[0118] S310. Based on the articulated vehicle steering model and the expected value of the steering curvature, calculate the expected value of the articulation angle using the gradient descent algorithm.

[0119] In this embodiment, the articulated vehicle steering model is used to describe the steering curvature. With hinge angle The mathematical expression for the articulated car steering model is given by formula (11):

[0120] (11)

[0121] Obtain the articulation angle function configured according to the articulated vehicle steering model, as shown in formula (12):

[0122] (12).

[0123] Obtain the cost function configured according to the articulated vehicle steering model, as shown in formula (13):

[0124] (13).

[0125] The expected value of the turning curvature Substitute the hinge angle function and use the cost function value With the goal of minimizing the hinge angle, the hinge angle function is iteratively solved using the gradient descent method to calculate the expected value of the hinge angle. The formula for calculating the expected value of the hinge angle is shown in formula (14):

[0126] (14);

[0127] in, Let be the iteration step size, when The iteration stops when the time is reached, and the hinge angle obtained in the last iteration is the expected value of the hinge angle.

[0128] S311. Generate steering drive command to control the steering motor to drive the hinge angle change to the desired hinge angle value.

[0129] In this embodiment, a steering drive command is generated based on the expected value of the hinge angle. The steering drive command is used to control the steering motor to drive the hinge angle to change to the expected value of the hinge angle, thereby realizing the articulated vehicle reference path tracking autonomous driving control.

[0130] As can be seen from the above technical solutions, the navigation control method for an articulated vehicle provided in this application calculates the control variable articulation angle based on the position and orientation information of the articulated vehicle and the deviation from the reference path. Due to the nonlinear characteristics of the hinge angle and position, an intermediate amount of turning curvature is introduced. Then, the control problem is transformed into a linear control problem, thereby enabling the calculation of the expected value of the steering curvature using the LQR algorithm. Further based on the turning curvature and hinge angle The articulation angle is solved by relational analysis. It is evident that this solution simplifies the automatic driving control method for articulated vehicle curve path tracking, improves the navigation control of the articulated vehicle, and reduces computational load and hardware requirements. For example, when this method is deployed on an NXPLPC4337, it takes only 6ms, significantly improving the navigation control efficiency of the articulated vehicle. By deploying the articulated vehicle navigation control method provided in this application on agricultural navigation products to achieve automatic driving control for articulated vehicle curve path tracking, the root mean square error of the tracking error is statistically analyzed in operations at speeds below 10km / h, and the target tracking accuracy is within ±5cm, indicating a small error.

[0131] Furthermore, compared to the traditional three-point curvature calculation method, which results in large fluctuations in the calculated curvature due to the imperfect smoothness of the planned path, leading to uneven vehicle control, this method provides a specific approach to calculate the steering curvature of the target reference point based on the pre-aiming reference point and the pre-aiming distance. This improves the smoothness of the steering curvature, thereby ensuring smooth vehicle control.

[0132] In summary, this solution addresses the practical application requirements of articulated vehicle curve path tracking. It linearizes the traditional articulated vehicle kinematic model, optimizes the solution of the nonlinear model, and integrates the LQR algorithm and a pure tracking algorithm to design an automatic control algorithm. This control method achieves target tracking accuracy within [specific parameters]. cm, with a small error.

[0133] The above describes a navigation control method for an articulated vehicle provided by an embodiment of this application. The following will describe the apparatus for performing the above-described navigation control method for an articulated vehicle.

[0134] Please see Figure 4 , Figure 4 This is a schematic diagram of the structure of a navigation control device for an articulated vehicle provided in an embodiment of this application. Figure 4 As shown, the navigation control device 400 of the articulated vehicle includes:

[0135] The navigation monitoring unit 401 is used to acquire the navigation information of the articulated vehicle in real time. The navigation information includes real-time pose information and speed. The real-time pose information includes the real-time values ​​of the X-axis coordinate, Y-axis coordinate, and heading angle of the articulated vehicle in the global coordinate system.

[0136] The curvature error monitoring unit 402 is used to calculate the steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model.

[0137] The discrete error tracking model is used to describe the linear relationship between the discrete pose tracking error vector and the steering curvature error. The pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinates of the articulated vehicle and the target reference point. The Y-axis tracking error is the difference between the Y-axis coordinates of the articulated vehicle and the target reference point. The heading angle tracking error is the difference between the heading angle of the articulated vehicle and the heading angle of the target reference point. The steering curvature error is the difference between the steering curvature of the articulated vehicle and the steering curvature of the target reference point.

[0138] Curvature prediction unit 403 is used to calculate the expected value of steering curvature based on the steering curvature error and the reference value of steering curvature;

[0139] The hinge angle prediction unit 404 is used to calculate the expected value of the hinge angle based on the expected value of the steering curvature.

[0140] The navigation control unit 405 is used to generate steering drive commands to control the steering motor to drive the hinge angle to change to the desired value of the hinge angle.

[0141] In one possible implementation, the navigation control unit of the articulated vehicle further includes: a model building unit, used for:

[0142] A motion state model of the articulated vehicle is constructed; the kinematic model of the articulated vehicle is used to describe the relationship between the pose vector and the steering curvature, wherein the pose vector is composed of the X-axis coordinate value, Y-axis coordinate value and heading angle of the articulated vehicle in the global coordinate system;

[0143] The motion state model is linearized to construct a continuous error tracking model for the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the pose tracking error vector and the steering curvature error in a continuous form.

[0144] Based on a preset sampling time, the continuous error tracking model is discretized to obtain the discrete error tracking model.

[0145] In one possible implementation, the curvature error monitoring unit, when calculating the steering curvature error based on the articulated vehicle's navigation information and a preset discrete error tracking model, specifically performs the following:

[0146] Based on the preset discrete error tracking model, the performance function of the linear quadratic regulator LQR is constructed.

[0147] Based on the navigation information of the articulated vehicle and the Ricardi equation, the LQR performance function is solved to obtain the optimal value of the steering curvature error.

[0148] In one possible implementation, the curvature prediction unit, used to calculate the expected value of the steering curvature based on the steering curvature error and the steering curvature of the target reference point, specifically uses the following for:

[0149] Based on the real-time pose information of the articulated vehicle, the target reference point is determined from the preset reference path, and the pose information of the target reference point is obtained. The pose information of the target reference point includes the X-axis coordinate value, Y-axis coordinate value, and heading angle of the target reference point in the global coordinate system.

[0150] The distances between reference points on the reference path located after the target reference point and the target reference point are calculated sequentially from near to far until the distance is greater than a preset aiming distance threshold, thus obtaining the aiming reference point and the aiming distance.

[0151] Based on the pose information of the pre-aiming reference point and the target reference point, the turning curvature of the target reference point on the reference path is calculated and used as a reference value for the turning curvature;

[0152] The sum of the optimal value of the steering curvature error and the reference value of the steering curvature is calculated as the expected value of the steering curvature.

[0153] In one possible implementation, when the hinge angle prediction unit is used to calculate the expected value of the hinge angle based on the expected value of the steering curvature, it is specifically used for:

[0154] Based on the preset articulated vehicle steering model and the expected value of the steering curvature, the gradient descent algorithm is used to calculate the expected value of the articulation angle. The articulated vehicle steering model is used to describe the relationship between the articulation angle and the steering curvature.

[0155] This application also provides an electronic device in its embodiments. (See reference...) Figure 5 The diagram illustrates a structural schematic suitable for implementing the electronic device in the embodiments of this application. The electronic device in the embodiments of this application may include, but is not limited to, fixed terminals such as mobile phones, laptops, PDAs (personal digital assistants), PADs (tablet computers), desktop computers, etc. Figure 5 The electronic device shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of this application.

[0156] like Figure 5 As shown, the electronic device may include a processing unit (e.g., a central processing unit, a graphics processing unit, etc.) 501, which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 502 or a program loaded from a storage device 508 into a random access memory (RAM) 503. When the electronic device is powered on, the RAM 503 also stores various programs and data required for the operation of the electronic device. The processing unit 501, ROM 502, and RAM 503 are interconnected via a bus 504. An input / output (I / O) interface 505 is also connected to the bus 504.

[0157] Typically, the following devices can be connected to I / O interface 505: input devices 506 including, for example, touchscreens, touchpads, keyboards, mice, cameras, microphones, accelerometers, gyroscopes, etc.; output devices 507 including, for example, liquid crystal displays (LCDs), speakers, vibrators, etc.; storage devices 508 including, for example, memory cards, hard drives, etc.; and communication devices 509. Communication device 509 allows electronic devices to communicate wirelessly or wiredly with other devices to exchange data. Although Figure 5 Electronic devices with various devices are shown, but it should be understood that it is not required to implement or have all of the devices shown. More or fewer devices may be implemented or have alternatively.

[0158] This application also provides a computer program product including computer-readable instructions, which, when executed on an electronic device, cause the electronic device to implement any of the articulated vehicle navigation control methods provided in this application.

[0159] This application also provides a computer-readable storage medium carrying one or more computer programs. When the one or more computer programs are executed by an electronic device, the electronic device can implement any of the navigation control methods for articulated vehicles provided in this application.

[0160] It should also be noted that the device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. In addition, in the device embodiment drawings provided in this application, the connection relationship between modules indicates that they have a communication connection, which can be implemented as one or more communication buses or signal lines.

[0161] Through the above description of the embodiments, those skilled in the art can clearly understand that this application can be implemented by means of software plus necessary general-purpose hardware, or it can be implemented by special-purpose hardware including application-specific integrated circuits, special-purpose CPUs, special-purpose memory, special-purpose components, etc. Generally, any function performed by a computer program can be easily implemented by corresponding hardware, and the specific hardware structure used to implement the same function can also be diverse, such as analog circuits, digital circuits, or special-purpose circuits. However, for this application, software program implementation is more often the preferred implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a readable storage medium, such as a computer floppy disk, USB flash drive, mobile hard disk, ROM, RAM, magnetic disk, or optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, training equipment, or network device, etc.) to execute the methods described in the various embodiments of this application.

[0162] In the above embodiments, the implementation can be achieved, in whole or in part, through software, hardware, firmware, or any combination thereof. When implemented in software, it can be implemented, in whole or in part, in the form of a computer program product.

[0163] The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of this application are generated. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions may be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions may be transmitted from one website, computer, training device, or data center to another website, computer, training device, or data center via wired (e.g., coaxial cable, fiber optic, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium may be any available medium that a computer can store or a data storage device such as a training device or data center that integrates one or more available media. The available media may be magnetic media (e.g., floppy disks, hard disks, magnetic tapes), optical media (e.g., DVDs), or semiconductor media (e.g., solid-state drives (SSDs)).

Claims

1. A navigation control method for an articulated vehicle, characterized in that, include: The navigation information of the articulated vehicle is acquired in real time. The navigation information includes real-time pose information and speed. The real-time pose information includes the real-time values ​​of the X-axis coordinate, Y-axis coordinate, and heading angle of the articulated vehicle in the global coordinate system. Based on the navigation information of the articulated vehicle and the preset discrete error tracking model, the steering curvature error is calculated; The discrete error tracking model is used to describe the linear relationship between the discrete pose tracking error vector and the steering curvature error. The pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinates of the articulated vehicle and the target reference point. The Y-axis tracking error is the difference between the Y-axis coordinates of the articulated vehicle and the target reference point. The heading angle tracking error is the difference between the heading angle of the articulated vehicle and the heading angle of the target reference point. The steering curvature error is the difference between the expected value of the steering curvature of the articulated vehicle and the steering curvature of the target reference point. Based on the steering curvature error and the steering curvature of the target reference point, the expected value of the steering curvature is calculated; Based on the expected value of the steering curvature, calculate the expected value of the hinge angle; Generate steering drive commands to control the steering motor to change the hinge angle to the desired value of the hinge angle.

2. The navigation control method for an articulated vehicle according to claim 1, characterized in that, The navigation control method for the articulated vehicle also includes: A motion state model of the articulated vehicle is constructed; the kinematic model of the articulated vehicle is used to describe the relationship between the pose vector and the steering curvature, wherein the pose vector is composed of the X-axis coordinate value, Y-axis coordinate value and heading angle of the articulated vehicle in the global coordinate system; The motion state model is linearized to construct a continuous error tracking model for the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the pose tracking error vector and the steering curvature error in a continuous form. Based on a preset sampling time, the continuous error tracking model is discretized to obtain the discrete error tracking model.

3. The navigation control method for an articulated vehicle according to claim 1, characterized in that, The calculation of steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model includes: Based on the preset discrete error tracking model, the performance function of the linear quadratic regulator LQR is constructed. Based on the navigation information of the articulated vehicle and the Ricardi equation, the LQR performance function is solved to obtain the optimal value of the steering curvature error.

4. The navigation control method for an articulated vehicle according to claim 1, characterized in that, The step of calculating the expected value of the steering curvature error based on the steering curvature error and the steering curvature of the target reference point includes: Based on the real-time pose information of the articulated vehicle, the target reference point is determined from the preset reference path, and the pose information of the target reference point is obtained. The pose information of the target reference point includes the X-axis coordinate value, Y-axis coordinate value, and heading angle of the target reference point in the global coordinate system. The distances between reference points on the reference path located after the target reference point and the target reference point are calculated sequentially from near to far until the distance is greater than a preset aiming distance threshold, thus obtaining the aiming reference point and the aiming distance. The steering curvature of the target reference point on the reference path is calculated based on the pose information of the pre-aiming reference point and the target reference point, and used as a reference value for the steering curvature; The sum of the optimal value of the steering curvature error and the reference value of the steering curvature is calculated as the expected value of the steering curvature.

5. The navigation control method for an articulated vehicle according to claim 1, characterized in that, The step of calculating the expected value of the hinge angle based on the expected value of the steering curvature includes: Based on the preset articulated vehicle steering model and the expected value of the steering curvature, the gradient descent algorithm is used to calculate the expected value of the articulation angle. The articulated vehicle steering model is used to describe the relationship between the articulation angle and the steering curvature.

6. A navigation control device for an articulated vehicle, characterized in that, include: The navigation monitoring unit is used to acquire the navigation information of the articulated vehicle in real time. The navigation information includes real-time pose information and speed. The real-time pose information includes the real-time values ​​of the X-axis coordinate, Y-axis coordinate, and heading angle of the articulated vehicle in the global coordinate system. The curvature error monitoring unit is used to calculate the steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model. The discrete error tracking model is used to describe the linear relationship between the discrete pose tracking error vector and the steering curvature error. The pose tracking error consists of X-axis tracking error, Y-axis tracking error, and heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinates of the articulated vehicle and the target reference point. The Y-axis tracking error is the difference between the Y-axis coordinates of the articulated vehicle and the target reference point. The heading angle tracking error is the difference between the heading angle of the articulated vehicle and the heading angle of the target reference point. The steering curvature error is the difference between the steering curvature of the articulated vehicle and the steering curvature of the target reference point. The curvature prediction unit is used to calculate the expected value of the steering curvature based on the steering curvature error and the reference value of the steering curvature. The hinge angle prediction unit is used to calculate the expected value of the hinge angle based on the expected value of the steering curvature. A navigation control unit is used to generate steering drive commands to control the steering motor to drive the hinge angle to change to the desired value of the hinge angle.

7. The navigation control device for an articulated vehicle according to claim 6, characterized in that, The navigation control device for the articulated vehicle further includes: a model building unit, used for: A motion state model of the articulated vehicle is constructed; the kinematic model of the articulated vehicle is used to describe the relationship between the pose vector and the steering curvature, wherein the pose vector is composed of the X-axis coordinate value, Y-axis coordinate value and heading angle of the articulated vehicle in the global coordinate system; The motion state model is linearized to construct a continuous error tracking model for the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the pose tracking error vector and the steering curvature error in a continuous form. Based on a preset sampling time, the continuous error tracking model is discretized to obtain the discrete error tracking model.

8. A computer program product, characterized in that, It includes computer-readable instructions that, when executed on an electronic device, cause the electronic device to implement the navigation control method for an articulated vehicle as described in any one of claims 1 to 5.

9. An electronic device, characterized in that, It includes at least one processor and a memory connected to the processor, wherein: The memory is used to store computer programs; The processor is used to execute the computer program to enable the electronic device to implement the navigation control method for the articulated vehicle as described in any one of claims 1 to 5.

10. A computer storage medium, characterized in that, The storage medium carries one or more computer programs that, when executed by an electronic device, enable the electronic device to implement the navigation control method for the articulated vehicle as described in any one of claims 1 to 5.

Images