Navigation control method of articulated vehicle and related device

Abstract

The invention discloses a navigation control method of an articulated vehicle and a related device, and relates to the field of automatic driving control, and the method comprises the following steps: obtaining navigation information of the articulated vehicle in real time, and calculating a steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model. An expected value of the steering curvature is calculated based on the steering curvature error and a reference value of the steering curvature. And calculating the expected value of the hinge angle according to the expected value of the steering curvature. And generating a steering driving instruction to control a steering motor to drive the hinge angle to change into an expected value of the hinge angle. The discrete error tracking model is used for describing a linear relation between a pose tracking error vector in a discrete form and a steering curvature error, and the pose tracking error is composed of an X-axis tracking error, a Y-axis tracking error and a course angle tracking error. The motion state model of the articulated vehicle is linearized, and the control precision of curve tracking of the articulated vehicle is improved.

Description

Technical Field

[0001] The present application relates to the field of automatic driving control technology, and in particular to a navigation control method and related devices for an articulated vehicle. Background Art

[0002] With the development of MESMS (Micro-Electro-Mechanical-System) sensors, navigation and control technology, precision agriculture is rapidly becoming a trend. The existing navigation system of agricultural machinery is mainly developed for front-wheel steering vehicles. When the existing navigation system is applied to articulated vehicles, the navigation control method has poor adaptability to articulated vehicles, resulting in low accuracy of navigation control of articulated vehicles. Summary of the invention

[0003] In view of the above problems, the present application provides a navigation control method and related devices for an articulated vehicle to achieve the purpose of improving the navigation control accuracy of the articulated vehicle. The specific scheme is as follows:

[0004] A first aspect of the present application provides a navigation control method for an articulated vehicle, comprising:

[0005] Acquire navigation information of the articulated vehicle in real time, the navigation information including real-time position information and speed, wherein the real-time position information includes the real-time value of the X-axis coordinate value, the real-time value of the Y-axis coordinate value, and the real-time value of the heading angle of the articulated vehicle in the global coordinate system;

[0006] Calculating a steering curvature error based on navigation information of the articulated vehicle and a preset discrete error tracking model;

[0007] Wherein, the discrete error tracking model is used to describe the linear relationship between the discrete form of the posture tracking error vector and the steering curvature error, the posture tracking error is composed of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error, the X-axis tracking error is the difference between the X-axis coordinate value of the articulated vehicle and the target reference point, the Y-axis tracking error is the difference between the Y-axis coordinate value of the articulated vehicle and the Y-axis coordinate value of 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, and 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] Calculating an expected value of the steering curvature based on the steering curvature error and the steering curvature of the target reference point;

[0009] Calculating an expected value of the articulation angle according to the expected value of the steering curvature;

[0010] A steering drive command is generated to control the steering motor to drive the articulation angle to change to a desired value of the articulation angle.

[0011] In a possible implementation, the navigation control method of the articulated vehicle further includes:

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

[0013] Linearizing the motion state model to construct a continuous error tracking model of the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the continuous posture tracking error vector and the steering curvature error;

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

[0015] In a possible implementation, calculating the steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model includes:

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

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

[0018] In a possible implementation, calculating an expected value of the steering curvature, the steering curvature error, based on the steering curvature error and the steering curvature of the target reference point includes:

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

[0020] Calculating 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 a preset preview distance threshold, thereby obtaining a preview reference point and a preview distance;

[0021] Calculating a turning curvature of the target reference point on the reference path based on the position information of the preview reference point and the target reference point as a reference value of 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 a possible implementation, calculating the expected value of the articulation angle according to the expected value of the steering curvature includes:

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

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

[0026] A navigation monitoring unit, used to obtain navigation information of the articulated vehicle in real time, wherein the navigation information includes real-time position information and speed, wherein the real-time position information includes the real-time value of the X-axis coordinate value, the real-time value of the Y-axis coordinate value, and the real-time value of the heading angle of the articulated vehicle in the global coordinate system;

[0027] a curvature error monitoring unit, configured to calculate a steering curvature error based on navigation information of the articulated vehicle and a preset discrete error tracking model;

[0028] Wherein, the discrete error tracking model is used to describe the linear relationship between the discrete form of the posture tracking error vector and the steering curvature error, the posture tracking error is composed of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error, the X-axis tracking error is the difference between the X-axis coordinate value of the articulated vehicle and the target reference point, the Y-axis tracking error is the difference between the Y-axis coordinate value of the articulated vehicle and the Y-axis coordinate value of 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, 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;

[0029] a curvature prediction unit, configured to calculate an expected value of the steering curvature based on the steering curvature error and a reference value of the steering curvature;

[0030] an articulation angle prediction unit, configured to calculate an expected value of the articulation angle according to the expected value of the steering curvature;

[0031] The navigation control unit is used to generate a steering drive instruction to control the steering motor to drive the articulation angle to change to the expected value of the articulation angle.

[0032] In a possible implementation, the navigation control device of the articulated vehicle further includes: a model building unit, configured to:

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

[0034] Linearizing the motion state model to construct a continuous error tracking model of the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the continuous posture tracking error vector and the steering curvature error;

[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 the present application provides a computer program product, comprising computer-readable instructions, which, when executed on an electronic device, enables the electronic device to implement the navigation control method for an articulated vehicle according to the first aspect or any implementation of the first aspect.

[0037] A fourth aspect of the present application provides an electronic device, comprising 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 of the articulated vehicle according to the first aspect or any implementation manner of the first aspect.

[0040] A fifth aspect of the present application provides a computer storage medium, which carries one or more computer programs. When the one or more computer programs are executed by an electronic device, the electronic device can implement the navigation control method of the articulated vehicle according to the first aspect or any implementation of the first aspect.

[0041] By means of the above technical scheme, the present application provides a navigation control method and related devices for an articulated vehicle, which obtain the navigation information of the articulated vehicle in real time, and calculate the steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model. Based on the steering curvature error and the reference value of the steering curvature, the expected value of the steering curvature is calculated. According to the expected value of the steering curvature, the expected value of the articulation angle is calculated. A steering drive instruction is generated to control the steering motor to drive the articulation angle to change to the expected value of the articulation angle. Among them, the navigation information includes real-time posture information and speed, and the real-time posture information includes the real-time value of the X-axis coordinate value, the real-time value of the Y-axis coordinate value, and the real-time value of the heading angle of the articulated vehicle in the global coordinate system. Since the discrete error tracking model is used to describe the linear relationship between the discrete form of the posture tracking error vector and the steering curvature error, the posture tracking error is composed of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinate value of the articulated vehicle and the target reference point, the Y-axis tracking error is the difference between the Y-axis coordinate value of the articulated vehicle and the Y-axis coordinate value of 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, 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. That is, based on the motion characteristics of the articulated vehicle, the intermediate steering curvature is introduced to linearize the motion state model of the articulated vehicle, overcome the problem of high computational difficulty caused by the nonlinear characteristics of the position and articulation angle of the articulated vehicle, and improve the control accuracy of the curve tracking of the articulated vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The above and other features, advantages and aspects of the embodiments of the present disclosure will become more apparent with reference to the following detailed description in conjunction with the accompanying drawings. Throughout the accompanying drawings, the same or similar reference numerals represent 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 schematic diagram of the steering of an articulated vehicle provided in an embodiment of the present application;

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

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

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

[0047] Figure 5 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0048] The following describes the embodiments of the present application in conjunction with the drawings in the embodiments of the present application. The terms used in the implementation method section of the present application are only used to explain the specific embodiments of the present application, and are not intended to limit the present application.

[0049] The embodiments of the present application are described below in conjunction with the accompanying drawings. Those skilled in the art will appreciate that, with the development of technology and the emergence of new scenarios, the technical solutions provided in the embodiments of the present application are also applicable to similar technical problems.

[0050] The terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and need not be used to describe a specific order or sequential order. It should be understood that the terms used in this way can be interchangeable under appropriate circumstances, which is only to describe the distinction mode adopted by the objects of the same attributes when describing in the embodiments of the present application. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, so that the process, method, system, product or equipment comprising a series of units need not be limited to those units, but may include other units that are not clearly listed or inherent to these processes, methods, products or equipment.

[0051] When controlling the navigation of an articulated vehicle (abbreviated as an articulated vehicle), it is necessary to control the steering of the articulated vehicle in real time, usually by controlling the articulation angle to control the steering of the articulated vehicle, so as to control the articulated vehicle to drive automatically along a reference path. Due to the different motion characteristics of different types of vehicles, and the nonlinear relationship between the articulation angle, a navigation control parameter of an articulated vehicle, and the position of the articulated vehicle, the existing navigation control system used on other types of vehicles has poor adaptability when applied to articulated vehicles.

[0052] In view of the nonlinear characteristics, the prior art determines the optimization function of the nonlinear model predictive control based on the planned segmented reference path, with the goal of optimizing the tracking effect of the articulated vehicle on the segmented reference path and the smoothness of the control variable output, and obtains the optimal control sequence so that the articulated vehicle can track the planned segmented reference path. The prior art has a large amount of calculation and high requirements for the hardware platform, which often leads to low control accuracy.

[0053] Especially in the actual autonomous driving process, there are complex control requirements such as irregular terrain, obstacle avoidance, U-turns, etc., 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 needs to be urgently solved by technical personnel in this field.

[0054] Figure 1 A schematic diagram of the steering of an articulated vehicle provided in an embodiment of the present application, such as Figure 1As shown, the articulated vehicle is formed by a tractor 101 and a trailer 102 connected by an articulated manner, wherein 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 It refers to the angle between the tractor and the trailer, that is, the difference between the heading angle of the tractor and the heading angle of the trailer. The size of the articulation angle directly reflects the relative position relationship between the tractor and the trailer.

[0055] An embodiment of the present application provides a navigation control method for an articulated vehicle. The method can be applied in the field of autonomous driving control, and specifically in an articulated vehicle autonomous driving scenario. The method tracks a reference path of the articulated vehicle and, based on the current posture information of the articulated vehicle, controls the navigation of the articulated vehicle by controlling the articulation angle, so that the articulated vehicle can automatically drive along the reference path.

[0056] It should be noted that this method can be applied to a vehicle controller of an articulated vehicle, in which a navigation control system is configured. Figure 2 , Figure 2 A flowchart of a navigation control method for an articulated vehicle provided in an embodiment of the present application is shown in FIG. Figure 2 As shown, the method may include S201 to S205, and these steps 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 posture information and speed, and the real-time posture information includes the real-time value of the X-axis coordinate value, the real-time value of the Y-axis coordinate value, and the real-time value of the 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 of the two antennas. At the same time, by receiving the differential data of the base station, real-time carrier phase differential positioning can be achieved, thereby obtaining centimeter-level navigation information.

[0060] It should be noted that the specific method for constructing the global coordinate system can be found in the prior art.

[0061] S202: Calculate the steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model.

[0062] Among them, the discrete error tracking model is used to describe the linear relationship between the discrete form of the posture tracking error vector and the steering curvature error. The posture tracking error is composed of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinate value of the articulated vehicle and the target reference point, the Y-axis tracking error is the difference between the Y-axis coordinate value of the articulated vehicle and the Y-axis coordinate value of 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, 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.

[0063] In an optional embodiment, the state equation expression of the discrete error tracking model is: ,in: represents the moment in discrete form, for The state variables at time , that is, The pose tracking error vector at time , for The state variable at the moment, that is The pose tracking error vector at time , for The state transfer matrix of the error tracking model at time instant, that is, the discrete form of the state transfer matrix, for The control variable at time The steering curvature error at time for The discrete form of the control matrix of the discrete error tracking model at time instant, T is the discrete time step, that is, the sampling time interval.

[0064] Specifically, the discrete forms of the state transfer matrix and the control matrix are .

[0065] In an optional embodiment, the motion state model of the articulated vehicle is linearized and discretized by introducing the steering curvature to obtain a discrete error tracking model. It can be understood that the state variable and the control variable in the discrete error tracking model are linearly related. The navigation information of the articulated vehicle is substituted into the discrete error tracking model, and the optimal value of the control variable under the navigation information is calculated as the steering curvature error.

[0066] S203: Calculate an expected value of the steering curvature based on the steering curvature error and the reference value of the 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, and the steering curvature error represents the difference between the steering curvature of the articulated vehicle and the steering curvature of the target reference point on the reference trajectory. 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. Therefore, the expected value of the steering curvature can be obtained by adding the steering curvature error to the reference value of the steering curvature.

[0068] S204: Calculate an expected value of the articulation angle according to 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, and the expected value of the articulation angle under the expected value of the steering curvature can be calculated through the relationship.

[0070] S205: Generate a steering drive instruction to control the steering motor to drive the articulation angle to change to a desired value of the articulation angle.

[0071] In this embodiment, the driving instruction is used to control the steering motor to drive the articulation angle to change to a desired value of the articulation angle, wherein the instruction interaction method between the navigation system and the steering motor refers to the prior art.

[0072] It can be seen from the above technical scheme that the navigation control method of an articulated vehicle provided in the embodiment of the present application obtains the navigation information of the articulated vehicle in real time, and calculates the steering curvature error based on the navigation information of the articulated vehicle and a preset discrete error tracking model. Based on the steering curvature error and the reference value of the steering curvature, the expected value of the steering curvature is calculated. According to the expected value of the steering curvature, the expected value of the articulation angle is calculated. A steering drive instruction is generated to control the steering motor to drive the articulation angle to change to the expected value of the articulation angle. Since the discrete error tracking model is used to describe the linear relationship between the discrete form of the posture tracking error vector and the steering curvature error, the posture tracking error is composed of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error. The X-axis tracking error is the difference between the X-axis coordinate value of the articulated vehicle and the target reference point, the Y-axis tracking error is the difference between the Y-axis coordinate value of the articulated vehicle and the Y-axis coordinate value of 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, 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. That is, based on the motion characteristics of the articulated vehicle, the intermediate steering curvature is introduced to linearize the motion state model of the articulated vehicle, overcome the problem of high computational difficulty caused by the nonlinear characteristics of the position and articulation angle of the articulated vehicle, and improve the control accuracy of the curve tracking of the articulated vehicle.

[0073] Furthermore, the present application provides a specific implementation method of a navigation control method for an articulated vehicle. Figure 3The specific implementation flow chart of the navigation control method for the articulated vehicle provided in the embodiment of the present application is as follows: Figure 3 As shown, this method specifically includes:

[0074] S301, constructing a motion state model of an articulated vehicle.

[0075] In this embodiment, the state variables of the kinematic model of the articulated vehicle are the coordinate values ​​of the X axis of the articulated vehicle in the global coordinate system. , Y-axis coordinate value and heading angle The pose vector formed is recorded as The control variable of the kinematic model of the articulated vehicle is the steering curvature k, denoted as The kinematic model of the articulated vehicle is used to describe the 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 formula (1):

[0077] (1).

[0078] in, It represents the linear velocity of the articulated vehicle in the X-axis direction in the global coordinate system. The linear velocity of the articulated vehicle in the Y-axis direction in the global coordinate system, represents the heading angular velocity of the articulated vehicle, Indicates the speed of the articulated vehicle. , , They are respectively the rate of change of the X-axis coordinate value, Y-axis coordinate value, and heading angle on the driving path.

[0079] S302: Linearize the motion state model to obtain a 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 the high-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 posture tracking error vector, and the control variable is the steering curvature error.

[0082] Among them, the posture tracking error vector is composed 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. The X-axis coordinate value of the target reference point The Y-axis tracking error is the Y-axis coordinate value of the articulated vehicle in the global coordinate system. The Y-axis coordinate value of the target reference point The heading angle tracking error is the heading angle of the articulated vehicle in the global coordinate system. Heading angle to the target reference point The steering curvature error is the steering curvature of the articulated vehicle in the global coordinate system. and the steering curvature of the target reference point The pose tracking error vector is recorded as , the control variable is recorded as .

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

[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 a 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: represents the moment in discrete form, for The state variables at time , that is, The pose tracking error vector at time , for The state variable at the moment, that is The pose tracking error vector at time , for The state transfer matrix of the error tracking model at time instant, that is, the discrete form of the state transfer matrix, for The control variable at time The steering curvature error at time for The discrete form of the control matrix of the discrete error tracking model at time instant, T is the discrete time step, that is, the sampling time interval.

[0090] Specifically, the discrete forms of the state transfer 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, and further discretizes the continuous error tracking model to obtain a discrete error tracking model, and deploys the discrete error tracking model in the vehicle controller.

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

[0094] In this embodiment, the navigation information includes real-time posture information and real-time speed, wherein the real-time posture information includes the real-time value of the X-axis coordinate value in the global coordinate system. And the real-time value of the Y-axis coordinate value and real-time value 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 of the two antennas. At the same time, by receiving the differential data of the base station, real-time carrier phase differential positioning can be achieved, thereby obtaining centimeter-level navigation information.

[0096] S305: Based on the real-time position and posture information of the articulated vehicle, determine the target reference point from the reference path, and obtain the position and posture 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 a 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 recorded as ( , , ),in, Indicates the X-axis coordinate value of the target reference point. Indicates the Y-axis coordinate value of the target reference point. Indicates the heading angle of the target reference point.

[0099] S306, calculating 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 a preset preview distance threshold, to obtain the preview reference point and the preview distance.

[0100] In this embodiment, the distance between the preview reference point and the target reference point is greater than the preset preview distance threshold, and the distance between the previous reference point of the preview reference point and the target reference point is less than the preview distance threshold. , and the distance between the preview reference point and the target reference point is taken as the preview distance.

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

[0102] (5)

[0103] The preview reference point is recorded as the nth reference point after the target reference point. , that is, the preview distance > .

[0104] S307. Calculate the turning curvature of the target reference point on the reference path based on the position and posture information of the preview reference point and the target reference point, as a reference value of the turning curvature.

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

[0106] (6).

[0107] S308. Construct an LQR performance function based on a discrete error tracking model.

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

[0109] (7).

[0110] in, is a semi-positive symmetric weighted matrix, is a semi-positive symmetric weighted matrix, and are the start and end time respectively. Formula (7) is a quadratic function, which represents a comprehensive performance index of control deviation and control stability. The performance is best when the solution is the smallest. The LQR control parameter is the steering curvature error .

[0111] S308: Based on the navigation information of the articulated vehicle and the Ricatti equation, solve the LQR performance function to obtain the optimal value of the steering curvature error. In this embodiment, the Ricatti equation is as follows:

[0112] (8)

[0113] Calculate the LQR control parameters using formula (8): , and obtain the optimal value of the steering curvature error:

[0114] (9)

[0115] S309 : Calculate an expected value of the steering curvature based on the optimal value of the steering curvature error and the reference value of the steering curvature.

[0116] Use formula (10) to calculate the expected value of the turning curvature: :

[0117] (10)

[0118] S310: Calculate the expected value of the articulation angle using a gradient descent algorithm according to the articulated vehicle steering model and the expected value of the steering curvature.

[0119] In this embodiment, the articulated vehicle steering model is used to describe the steering curvature. With articulation angle The mathematical expression of the articulated vehicle steering model is as follows:

[0120] (11)

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

[0122] (12).

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

[0124] (13).

[0125] The expected value of the turning curvature Substitute the hinge angle function and use the cost function value The minimum is the goal, and the articulation angle function is iteratively solved by the gradient descent method to calculate the expected value of the articulation angle. The calculation formula of the expected value of the articulation angle is as shown in formula (14):

[0126] (14);

[0127] in, is the iteration step length, when The iteration stops when , and the hinge angle obtained in the last iteration is the expected value of the hinge angle.

[0128] S311. Generate a steering drive instruction to control the steering motor to drive the articulation angle to change to a desired value of the articulation angle.

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

[0130] It can be seen from the above technical solution that the navigation control method of an articulated vehicle provided in the embodiment of the present application calculates the control variable articulation angle according to the deviation between the posture information of the articulated vehicle and the reference path. Due to the nonlinear characteristics of the articulation angle and position, the intermediate steering curvature is introduced After that, the control problem is transformed into a linear control problem, so that the expected value of the steering curvature can be calculated using the LQR algorithm. , further according to the turning curvature and articulation angle The relationship is used to solve the articulation angle. It can be seen that this solution can simplify the automatic driving control method for curved path tracking of articulated vehicles, improve the navigation control of articulated vehicles, reduce the amount of calculation and hardware requirements. For example, this method is deployed on NXP LPC4337 and executed in only 6ms, thereby improving the navigation control efficiency of the articulated vehicle. Automatic driving control of curved path tracking of articulated vehicles is achieved by deploying the navigation control method for articulated vehicles provided in this application on agricultural navigation products. The root mean square error of the tracking error is statistically analyzed in operations at speeds below 10 km / h, and the target tracking accuracy is ±5 cm, with a relatively small error.

[0131] Furthermore, compared with the traditional three-point curvature calculation method, which causes large fluctuations in the calculated curvature due to the incomplete smoothness of the planned path, thus causing uneven vehicle control, the present method provides a specific method for calculating the steering curvature of the target reference point based on the preview reference point and the preview distance, thereby improving the smoothness of the steering curvature and ensuring smooth vehicle control.

[0132] In summary, this solution aims at the needs of articulated vehicle curve path tracking in practical applications, linearizes the traditional articulated vehicle kinematic model, optimizes the solution of nonlinear model, and integrates LQR algorithm and pure tracking algorithm to design an automatic control algorithm. This control method controls the target tracking accuracy within cm, the error is small.

[0133] A navigation control method for an articulated vehicle provided in an embodiment of the present application is introduced above, and a device for executing the navigation control method for an articulated vehicle will be introduced below.

[0134] See also 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 the present application. Figure 4 As shown, the navigation control device 400 of the articulated vehicle includes:

[0135] The navigation monitoring unit 401 is used to obtain the navigation information of the articulated vehicle in real time, wherein the navigation information includes real-time position information and speed, wherein the real-time position information includes the real-time value of the X-axis coordinate value, the real-time value of the Y-axis coordinate value, and the real-time value of the heading angle of the articulated vehicle in the global coordinate system;

[0136] A curvature error monitoring unit 402, configured to calculate a steering curvature error based on navigation information of the articulated vehicle and a preset discrete error tracking model;

[0137] Wherein, the discrete error tracking model is used to describe the linear relationship between the discrete form of the posture tracking error vector and the steering curvature error, the posture tracking error is composed of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error, the X-axis tracking error is the difference between the X-axis coordinate value of the articulated vehicle and the target reference point, the Y-axis tracking error is the difference between the Y-axis coordinate value of the articulated vehicle and the Y-axis coordinate value of 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, 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;

[0138] a curvature prediction unit 403, configured to calculate an expected value of the steering curvature based on the steering curvature error and a reference value of the steering curvature;

[0139] an articulation angle prediction unit 404, configured to calculate an expected value of the articulation angle according to the expected value of the steering curvature;

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

[0141] In a possible implementation, the navigation control device of the articulated vehicle further includes: a model building unit, configured to:

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

[0143] Linearizing the motion state model to construct a continuous error tracking model of the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the continuous posture tracking error vector and the steering curvature error;

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

[0145] In a possible implementation, 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, specifically to:

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

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

[0148] In a possible implementation, the curvature prediction unit is used to calculate the expected value of the steering curvature, i.e., the steering curvature error, based on the steering curvature error and the steering curvature of the target reference point, and is specifically used to:

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

[0150] Calculating 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 a preset preview distance threshold, thereby obtaining a preview reference point and a preview distance;

[0151] Calculating a turning curvature of the target reference point on the reference path based on the position information of the preview reference point and the target reference point as a reference value of 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 a possible implementation, the articulation angle prediction unit is used to calculate the expected value of the articulation angle according to the expected value of the steering curvature, specifically to:

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

[0155] The present application also provides an electronic device in an embodiment. Figure 5 As shown, it shows a schematic diagram of the structure of an electronic device suitable for implementing the embodiment of the present application. The electronic device in the embodiment of the present 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 bring any limitation to the functions and scope of use of the embodiments of the present application.

[0156] like Figure 5 As shown, the electronic device may include a processing device (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 to a random access memory (RAM) 503. When the electronic device is powered on, various programs and data required for the operation of the electronic device are also stored in the RAM 503. The processing device 501, the ROM 502, and the RAM 503 are connected to each other via a bus 504. An input / output (I / O) interface 505 is also connected to the bus 504.

[0157] Typically, the following devices may be connected to the I / O interface 505: an input device 506 including, for example, a touch screen, a touch pad, a keyboard, a mouse, a camera, a microphone, an accelerometer, a gyroscope, etc.; an output device 507 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; a storage device 508 including, for example, a memory card, a hard disk, etc.; and a communication device 509. The communication device 509 may allow the electronic device to communicate with other devices wirelessly or by wire to exchange data. Although Figure 5 An electronic device having various devices is shown, but it should be understood that it is not required to implement or possess all the devices shown. More or fewer devices may be implemented or possessed instead.

[0158] Also provided in an embodiment of the present application is a computer program product including computer-readable instructions. When the computer-readable instructions are executed on an electronic device, the electronic device implements any one of the navigation control methods for an articulated vehicle provided in an embodiment of the present application.

[0159] A computer-readable storage medium is also provided in an embodiment of the present application. The storage medium carries one or more computer programs. When the one or more computer programs are executed by an electronic device, the electronic device can implement any one of the navigation control methods for the articulated vehicle provided in the embodiment of the present application.

[0160] It should also be noted that the device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed over multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. In addition, in the drawings of the device embodiments provided by the present application, the connection relationship between the modules indicates that there is a communication connection between them, which may be specifically implemented as one or more communication buses or signal lines.

[0161] Through the description of the above implementation mode, the technicians in the field can clearly understand that the present application can be implemented by means of software plus necessary general hardware, and of course, it can also be implemented by special hardware including special integrated circuits, special CPUs, special memories, special components, etc. In general, all functions completed by computer programs can be easily implemented by corresponding hardware, and the specific hardware structure used to implement the same function can also be various, such as analog circuits, digital circuits or special circuits. However, for the present application, software program implementation is a better implementation mode in more cases. Based on such an understanding, the technical solution of the present application is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a readable storage medium, such as a computer floppy disk, a U disk, a mobile hard disk, a ROM, a RAM, a disk or an optical disk, etc., including a number of instructions to enable a computer device (which can be a personal computer, a training device, or a network device, etc.) to execute the methods described in each embodiment of the present application.

[0162] In the above embodiments, all or part of the embodiments may be implemented by software, hardware, firmware or any combination thereof. When implemented by software, all or part of the embodiments may be implemented 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, the process or function described in the embodiment of the present application is generated in whole or in part. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions may be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions may be transmitted from a website site, a computer, a training device, or a data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) mode to another website site, computer, training device, or data center. 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, a data center, etc. that includes one or more available media integrations. The available medium may be a magnetic medium, (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid-state drive (SSD)), etc.

Claims

1. A navigation control method for an articulated vehicle, characterized in that: include: Acquire navigation information of the articulated vehicle in real time, the navigation information including real-time position information and speed, wherein the real-time position information includes the real-time value of the X-axis coordinate value, the real-time value of the Y-axis coordinate value, and the real-time value of the heading angle of the articulated vehicle in the global coordinate system; Calculating a steering curvature error based on navigation information of the articulated vehicle and a preset discrete error tracking model; Wherein, the discrete error tracking model is used to describe the linear relationship between the discrete form of the posture tracking error vector and the steering curvature error, the posture tracking error is composed of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error, the X-axis tracking error is the difference between the X-axis coordinate value of the articulated vehicle and the target reference point, the Y-axis tracking error is the difference between the Y-axis coordinate value of the articulated vehicle and the Y-axis coordinate value of 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, and 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; Calculating an expected value of the steering curvature based on the steering curvature error and the steering curvature of the target reference point; Calculating an expected value of the articulation angle according to 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 a desired value of the articulation angle.

2. The navigation control method of the articulated vehicle according to claim 1, characterized in that: The navigation control method of the articulated vehicle also includes: Constructing a motion state model of the articulated vehicle; the kinematic model of the articulated vehicle is used to describe the relationship between a posture vector and a steering curvature, wherein the posture vector is composed of an X-axis coordinate value, a Y-axis coordinate value and a heading angle of the articulated vehicle in a global coordinate system; Linearizing the motion state model to construct a continuous error tracking model of the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the continuous posture tracking error vector and the steering curvature error; 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 of an articulated vehicle according to claim 1, characterized in that: The calculating of the 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 linear quadratic regulator LQR performance function is constructed; Based on the navigation information of the articulated vehicle and the Riccati equation, the LQR performance function is solved to obtain the optimal value of the steering curvature error.

4. The navigation control method of an articulated vehicle according to claim 1, characterized in that: The step of calculating the expected value of the steering curvature, i.e., the steering curvature error, based on the steering curvature error and the steering curvature of the target reference point comprises: Based on the real-time posture information of the articulated vehicle, the target reference point is determined from a preset reference path, and the posture information of the target reference point is obtained, where the posture information of the target reference point includes an X-axis coordinate value, a Y-axis coordinate value, and a heading angle of the target reference point in a global coordinate system; Calculating 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 a preset preview distance threshold, thereby obtaining a preview reference point and a preview distance; Calculating a turning curvature of the target reference point on the reference path based on the position information of the preview reference point and the target reference point as a reference value of the turning 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 of an articulated vehicle according to claim 1, characterized in that: Calculating the expected value of the articulation angle according to the expected value of the steering curvature includes: According to a preset articulated vehicle steering model and the expected value of the steering curvature, a gradient descent algorithm is used to calculate the expected value of the articulation angle, wherein 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: A navigation monitoring unit, used to obtain navigation information of the articulated vehicle in real time, wherein the navigation information includes real-time position information and speed, wherein the real-time position information includes the real-time value of the X-axis coordinate value, the real-time value of the Y-axis coordinate value, and the real-time value of the heading angle of the articulated vehicle in the global coordinate system; a curvature error monitoring unit, configured to calculate a steering curvature error based on navigation information of the articulated vehicle and a preset discrete error tracking model; Wherein, the discrete error tracking model is used to describe the linear relationship between the discrete form of the posture tracking error vector and the steering curvature error, the posture tracking error is composed of the X-axis tracking error, the Y-axis tracking error, and the heading angle tracking error, the X-axis tracking error is the difference between the X-axis coordinate value of the articulated vehicle and the target reference point, the Y-axis tracking error is the difference between the Y-axis coordinate value of the articulated vehicle and the Y-axis coordinate value of 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, 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; a curvature prediction unit, configured to calculate an expected value of the steering curvature based on the steering curvature error and a reference value of the steering curvature; an articulation angle prediction unit, configured to calculate an expected value of the articulation angle according to the expected value of the steering curvature; The navigation control unit is used to generate a steering drive instruction to control the steering motor to drive the articulation angle to change to the expected value of the articulation angle.

7. The navigation control device for an articulated vehicle according to claim 6, characterized in that: The navigation control device of the articulated vehicle further comprises: a model building unit, which is used for: Constructing a motion state model of the articulated vehicle; the kinematic model of the articulated vehicle is used to describe the relationship between a posture vector and a steering curvature, wherein the posture vector is composed of an X-axis coordinate value, a Y-axis coordinate value and a heading angle of the articulated vehicle in a global coordinate system; Linearizing the motion state model to construct a continuous error tracking model of the articulated vehicle; the continuous error tracking model is used to describe the linear relationship between the continuous posture tracking error vector and the steering curvature error; 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 comprises computer-readable instructions, and when the computer-readable instructions are executed on an electronic device, the electronic device implements the navigation control method for an articulated vehicle as claimed in any one of claims 1 to 5.

9. An electronic device, characterized in that: The method comprises 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 so that the electronic device can implement the navigation control method of 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, and when the one or more computer programs are executed by an electronic device, the electronic device can implement the navigation control method for the articulated vehicle as described in any one of claims 1 to 5.

Images