Unstructured road path tracking and speed optimization control method, equipment, medium and product

Abstract

The invention discloses an unstructured road path tracking and speed optimization control method and device, a medium and a product, and relates to the technical field of vehicle automatic driving, and the method comprises the steps: obtaining the current state information of a vehicle and a plurality of path points; carrying out smoothing processing on the path points; selecting a target path point and a plurality of continuous path points including the target path point, generating a preview interval, and determining the curvature of the preview interval; obtaining a reference control quantity based on the current state information of the vehicle and the curvature of the preview interval; acquiring state information of a plurality of continuous path points, and determining a reference state quantity of a target path point; constructing a linear error equation and a cost function; determining an LQR feedback gain matrix by adopting a mixed solution mechanism and combining a cost function; and obtaining a final control instruction based on the LQR feedback gain matrix so as to control the vehicle to run to the target path point. According to the method, the efficient and stable vehicle control effect can be achieved, and the vehicle control performance is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of vehicle automatic driving, in particular to a non-structured road path tracking and speed optimization control method, device, medium and product. BACKGROUND

[0002] In the environment of non-structured roads (such as mountainous areas, hilly areas and other roads without obvious lane markings), the trajectory tracking control of vehicles faces many challenges. The existing technology usually relies on lane markings or high-precision maps, but it is easy to fail in the non-marked environment, resulting in a decrease in trajectory tracking accuracy. In addition, the common pure tracking algorithm (Pure Pursuit) and PID control method are difficult to balance stability and control accuracy, and are prone to overshoot, oscillation or trajectory deviation in complex terrain. Although the nonlinear model predictive control (NMPC) algorithm can provide better trajectory tracking effect, it has a large amount of calculation and is difficult to meet the real-time requirement. Moreover, the existing method has poor adaptability to speed control, and is difficult to dynamically adjust the speed according to the complex terrain features such as curves and slopes, which is easy to cause unstable vehicle driving or control performance degradation. SUMMARY

[0003] The purpose of the present application is to provide a non-structured road path tracking and speed optimization control method, device, medium and product, which can realize efficient and stable vehicle control effect.

[0004] To achieve the above purpose, the present application provides the following solutions:

[0005] In a first aspect, the present application provides a non-structured road path tracking and speed optimization control method, comprising:

[0006] Obtaining the current state information of the vehicle, and obtaining a plurality of path points from the planned path of the non-structured road to be driven by the vehicle;

[0007] Smoothing the path points to obtain smoothed path points;

[0008] Selecting one path point from the smoothed path points as a target path point, and selecting a plurality of continuous path points containing the target path point from the smoothed path points to generate a preview interval, and determining the curvature of the preview interval;

[0009] Obtaining the reference control amount based on the current state information of the vehicle and the curvature of the preview interval;

[0010] Obtaining the state information of a plurality of continuous path points, and determining the reference state amount of the target path point based on the state information of a plurality of continuous path points;

[0011] constructing a linear error equation based on the vehicle current state information, the reference state quantity of the target path point and the reference control quantity;

[0012] constructing a cost function based on the linear error equation;

[0013] adopting a hybrid solving mechanism to determine an LQR feedback gain matrix in combination with the cost function;

[0014] obtaining an LQR control instruction based on the LQR feedback gain matrix;

[0015] obtaining a final control instruction based on the LQR control instruction to control the vehicle to reach the target path point.

[0016] Optionally, the reference control quantity is determined based on the vehicle current state information and the curvature of the preview interval, comprising:

[0017] adopting a progressive start-stop strategy to determine a vehicle desired speed based on the vehicle current state information and the curvature of the preview interval; the progressive start-stop strategy comprises a vehicle start speed function and a vehicle stop speed function;

[0018] obtaining the reference control quantity based on the vehicle desired speed.

[0019] Optionally, the vehicle desired speed is determined based on the vehicle current state information and the curvature of the preview interval by adopting a progressive start-stop strategy, comprising:

[0020] determining a fused curvature based on the vehicle current state information and the curvature of the preview interval;

[0021] adopting a progressive start-stop strategy to obtain a vehicle speed in a start or stop phase based on the vehicle current state information;

[0022] determining the vehicle desired speed based on the vehicle current state information, the fused curvature and the vehicle speed in the start or stop phase.

[0023] Optionally, the linear error equation is constructed based on the vehicle current state information, the reference state quantity of the target path point and the reference control quantity, comprising:

[0024] constructing a vehicle kinematics equation based on the vehicle current state information;

[0025] obtaining an error state quantity and an error control quantity based on the vehicle current state information, the reference state quantity of the target path point and the reference control quantity;

[0026] obtaining the linear error equation based on the vehicle kinematics equation, the error state quantity and the error control quantity.

[0027] Optionally, the hybrid solving mechanism comprises querying a hash table and iteratively optimizing a cost function;

[0028] The hybrid solving mechanism is adopted to determine the LQR feedback gain matrix in combination with the cost function, comprising:

[0029] A heading angle index threshold and a speed index threshold are set;

[0030] The vehicle state in which the heading angle of the target path point does not exceed the heading angle index threshold and the vehicle desired speed does not exceed the speed index threshold is taken as a typical working condition;

[0031] A feedback matrix of the typical working condition is determined, and a hash table is constructed;

[0032] When the heading angle of the target path point does not exceed the heading angle index threshold and the vehicle desired speed does not exceed the speed index threshold, the feedback matrix is directly queried in the hash table as the LQR feedback gain matrix;

[0033] When the heading angle of the target path point exceeds the heading angle index threshold or the vehicle desired speed exceeds the speed index threshold, a Riccati equation is obtained according to a cost function and the linear error equation, and an iteration range is set;

[0034] According to the iteration range, the Riccati equation is iteratively updated;

[0035] The LQR feedback gain matrix is determined according to the updated Riccati equation.

[0036] Optionally, the LQR control instruction is obtained based on the LQR feedback gain matrix, comprising:

[0037] Based on the target path point and the current state information of the vehicle, a current error state quantity is determined;

[0038] The LQR control instruction is obtained based on the current error state quantity and the LQR feedback gain matrix.

[0039] Optionally, the LQR control instruction is obtained based on the LQR feedback gain matrix, comprising:

[0040] Based on the current state information of the vehicle and the linear error equation, future state information of the vehicle is predicted;

[0041] The LQR control instruction is obtained based on the future state information of the vehicle and the LQR feedback gain matrix.

[0042] In a second aspect, the present application provides a computer device, comprising: a memory, a processor to store a computer program on the memory and run the computer program on the processor, and the processor executes the computer program to implement the steps of the non-structured road path tracking and speed optimization control method according to any one of the above.

[0043] In a third aspect, the present application provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the steps of the non-structured road path tracking and speed optimization control method according to any one of the above.

[0044] In a fourth aspect, the present application provides a computer program product, comprising a computer program, and the computer program is executed by a processor to implement the steps of the non-structured road path tracking and speed optimization control method according to any one of the above.

[0045] According to the specific embodiments provided by the present application, the present application has the following technical effects:

[0046] The present application provides a non-structured road path tracking and speed optimization control method, device, medium and product, by obtaining a plurality of path points from the planned path of the non-structured road to be traveled by the vehicle, and smoothing the path points according to the current state information of the vehicle to obtain smoothed path points, and performing path tracking according to the obtained smoothed path points to improve the real-time performance and accuracy of path tracking. One path point is selected from the smoothed path points as a target path point; a plurality of continuous path points containing the target path point are obtained to generate a preview interval, and the curvature of the preview interval is determined; the reference control amount is obtained based on the current state information of the vehicle and the curvature of the preview interval, to improve the adaptability and accuracy of speed optimization control. The reference state amount of the target path point is determined according to the state information of the plurality of continuous path points; and a linear error equation is constructed based on the current state information of the vehicle, the reference state amount of the target path point and the reference control amount; a cost function is constructed based on the linear error equation; a hybrid solving mechanism is used to determine the LQR feedback gain matrix in combination with the cost function; the LQR control instruction is obtained based on the LQR feedback gain matrix, to dynamically adjust the control instruction according to the complex terrain features such as curves and slopes in the non-structured road contained in the smoothed path points, and the final control instruction is obtained based on the LQR control instruction, to efficiently control the vehicle to smoothly travel to the target path point. Thus, real-time and accurate path tracking and speed optimization control are achieved, efficient and smooth vehicle control effect is achieved, and vehicle control performance is improved. BRIEF DESCRIPTION OF DRAWINGS

[0047] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed in the embodiments. Obviously, the drawings described below only constitute some embodiments of the present application, and for those skilled in the art, other drawings can be obtained from these drawings without creative labor.

[0048] Figure 1 A flowchart of a non-structured road path tracking and speed optimization control method in an embodiment of the present application is shown.

[0049] Figure 2 A structural diagram of a computer device provided in an embodiment of the present application is shown. DETAILED DESCRIPTION

[0050] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments only constitute some embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0051] The above purposes, features and advantages of the present application can be more obvious and easy to understand. The present application will be further described in detail below with reference to the drawings and specific embodiments.

[0052] In an exemplary embodiment, as shown in Figure 1 A non-structured road path tracking and speed optimization control method is provided, which is executed by a computer device, specifically, can be executed by a terminal or a server, or can be executed by a terminal and a server together, for example, can be executed by a vehicle control device. In the embodiments of the present application, the method is taken as an example to track the original discrete path of the non-structured road output by the upstream planning module and optimize the speed control of the vehicle driving, which includes:

[0053] In step 100, the current state information of the vehicle is obtained, and a plurality of path points are obtained from the planned path of the non-structured road to be driven by the vehicle. The path points are smoothed to obtain smoothed path points.

[0054] In step 200, a path point is selected as a target path point from the smoothed path points, and a plurality of continuous path points containing the target path point are selected from the smoothed path points to generate a preview interval, and the curvature of the preview interval is determined. The reference control quantity is obtained based on the current state information of the vehicle and the curvature of the preview interval.

[0055] Step 300: Obtain the state information of multiple consecutive path points containing the target path point, and determine the reference state quantity of the target path point based on the state information of multiple consecutive path points containing the target path point.

[0056] Step 400: Construct a linear error equation based on the vehicle's current state information, the reference state variables of the target path point, and the reference control variables. Construct a cost function based on the linear error equation. Use a hybrid solution mechanism, combining the cost function to determine the LQR feedback gain matrix.

[0057] Step 500: Obtain LQR control commands based on the LQR feedback gain matrix. Obtain final control commands based on the LQR control commands to control the vehicle to reach the target path point.

[0058] As an optional implementation, to provide the LQR controller with a differentiable and continuous reference trajectory, ensure the smoothness of the final control command, and improve the continuity and trackability of the trajectory, step 100 uses a cubic spline interpolation function to smooth the path points, obtaining smoothed path points. For example, from the original discrete path points output by the upstream planning module, M path points are selected according to the principle of equal intervals to ensure the uniformity of spline node distribution and improve interpolation stability.

[0059] P i =(x i ,y i ), i = 0, 1, ..., M-1.

[0060] In the formula, P i Let (x) be the i-th path point selected from the original path. i ,y i ) represents the x and y coordinates of the path point.

[0061] Using the starting point of the path among the original discrete path points as the starting point of the arc length, calculate the cumulative arc length s of each path point. i , with s i Construct an M-1 segment cubic spline interpolation function for the parameters. The i-th segment spline interpolation function is expressed as:

[0062]

[0063] Where, s∈[s i ,s i+1 ]. x i (s) is the spline interpolation function of x with respect to the arc length s, y i (s) is the spline interpolation function of y with respect to the arc length s, where x is the independent variable. Let be the coefficients of the spline interpolation function for the i-th segment.

[0064] From the starting point of the path to the selected path point (x i,y i arc length s i As parameters, construct parametric equations x for each spline segment. i (s i ), y i (s i At the (i+1)th path point, the cubic spline interpolation function needs to satisfy the continuity of position, velocity, and acceleration, i.e., the constraints of the cubic spline interpolation function:

[0065] x i (s i+1 )=x i+1 (s i+1 ).

[0066] x′ i (s i+1 )=x′ i+1 (s i+1 ).

[0067] x″ i (s i+1 )=x″ i+1 (s i+1 ).

[0068] y i (s i+1 )=y i+1 (s i+1 ).

[0069] y′ i (s i+1 )=y′ i+1 (s i+1 ).

[0070] y″ i (s i+1 )=y″ i+1 (s i+1 ).

[0071] Where, x' i (s i+1 ), y' i (s i+1 ) is the spline interpolation function x i (s i ), y i (s i At connection point s i+1 The first derivative at x″. i (s i+1 ), y″ i (s i+1 ) is the spline interpolation function x i (s i ), y i(s i At connection point s i+1 The second derivative at point .

[0072] The above constraints are transformed into a system of linear equations to be solved, yielding all coefficients {a}. i ,b i ,c i ,d i Then, the smooth path points are calculated based on the cubic spline interpolation function:

[0073]

[0074] In the formula, Let x(s) represent the j-th smooth path point. j ),y(s j The coordinates of the smoothed path point are y and y. This smooths the original path points, eliminates abrupt curvature changes in the original discrete path, provides a differentiable and continuous reference trajectory for the LQR controller, and ensures the smoothness of control commands.

[0075] As an optional implementation, to provide a dynamic reference for the LQR controller and improve the adaptability of speed control, the process of obtaining the reference control quantity based on the current vehicle state information and the curvature of the pre-aiming interval in step 200 includes: using a progressive start-stop strategy to determine the desired vehicle speed based on the current vehicle state information and the curvature of the pre-aiming interval. The progressive start-stop strategy includes a vehicle start speed function and a vehicle stop speed function. The reference control quantity is obtained based on the desired vehicle speed. Among them, using a progressive start-stop strategy to determine the desired vehicle speed based on the current vehicle state information and the curvature of the pre-aiming interval includes: (1) determining the fusion curvature based on the current vehicle state information and the curvature of the pre-aiming interval. (2) using a progressive start-stop strategy to obtain the vehicle speed during the start-up or stop phase based on the current vehicle state information. (3) determining the desired vehicle speed based on the current vehicle state information, the fusion curvature, and the vehicle speed during the start-up or stop phase.

[0076] For example, based on the smoothed path points obtained in step 100, the desired vehicle speed v is calculated by comprehensively considering the curvature of the path formed by the smoothed path points, the vehicle's start-stop status, and the forward-looking information. desAnd the gradual start-stop strategy is adopted to optimize the speed change curve. From the l continuous path points containing the target path point m (that is, one path point in the above smooth path points is selected as the target path point, denoted as m, which represents the number of the target path point in the above smooth path points. For example, the 5th smooth path point is selected as the target path point, and m is 5), a preview interval is generated. It should be noted that the l continuous path points containing the target path point m can be selected to generate the preview interval, or the l continuous path points containing the target path point m can be selected to generate the preview interval. In this embodiment, the l continuous path points containing the target path point m are selected to generate the preview interval.

[0077] In order to suppress the influence of local curvature mutation in the path composed of smooth path points on speed optimization, a multi-source fusion strategy is adopted for curvature. The multi-source curvature fusion strategy for fusing curvature k is represented as:

[0078] k = max (αk1, βk2, γk3).

[0079] Wherein, k1 represents the curvature of the target path point; k2 represents the curvature of the vehicle actually executed, ω actual represents the angular velocity of the vehicle actually executed, v actual represents the speed of the vehicle actually executed; k3 represents the average curvature of the preview interval (i.e. the preview interval curvature), κ j represents the curvature of the jth smooth path point in the preview interval; α + β + γ = 1, and α, β, γ are weights (representing the importance of different curvatures).

[0080] In order to improve the safety and comfort of the vehicle, the gradual start-stop strategy is adopted, and the speed of the vehicle in the starting or stopping stage is obtained based on the current state information of the vehicle.

[0081] When the vehicle is in the starting stage, the acceleration is smoothed by using a quadratic function:

[0082]

[0083] Wherein, v start (t) is a function of the starting stage speed and time, T acc is the total time of the starting stage acceleration, t is the time spent from the start of the starting (t ≤ T acc ), v ref represents the current speed of the vehicle (i.e. the basic reference speed), and v start represents the speed of the starting stage.

[0084] When the vehicle is in the stopping stage, the vehicle speed is linearly attenuated according to the remaining distance from the position of the vehicle to the target path point:

[0085]

[0086] where d remain represents the remaining distance of the vehicle to the target path point, d slow is the distance at which the vehicle starts to decelerate, v stop represents the speed in the parking phase. It is to be noted that the speed of the vehicle is constantly changing when the vehicle is in the starting phase and the parking phase.

[0087] Based on the current state information of the vehicle, the fused curvature and the speed of the vehicle in the starting or parking phase, the expected speed v des of the vehicle is determined.

[0088]

[0089] where v ref is the basic reference speed (i.e. the current speed of the vehicle), a lat,max is the maximum allowed lateral acceleration, k represents the fused curvature obtained by the multi-source curvature strategy, v start / v stop is the speed of the vehicle in the starting or parking phase.

[0090] According to the expected speed of the vehicle, the reference control quantity u des is obtained, u des = [v des , ω des ] T , ω des = v des x k1. Where ω des represents the expected angular speed of the vehicle.

[0091] As an optional implementation, in order to improve the accuracy and stability of the speed optimization control, the process of constructing the linear error equation based on the current state information of the vehicle, the reference state quantity of the target path point and the reference control quantity in step 400 includes:

[0092] In step 411, a vehicle kinematics equation is constructed based on the current state information of the vehicle. For example, the constructed vehicle kinematics equation can be represented as: where [x, y, θ] T represents the current position and heading angle of the vehicle, is the first derivative of [x, y, θ] T ; ω ref represents the current angular speed of the vehicle (i.e. the basic reference angular speed).

[0093] In step 412, error state quantities and error control quantities are obtained based on the current state information of the vehicle, the reference state quantity of the target path point and the reference control quantity. For example, the error state quantity Xe may be expressed as: error control variable u e may be expressed as: In the formula, [x ref , y ref , θ ref ] T represents the reference state variable of the target path point (i.e., the position and heading angle of the target path point m, wherein θ ref = is obtained, (x m+1 , y m+1 ) represents the position of the m+1th smoothed path point, i.e., the position of the smoothed path point after the target path point), [v des , ω des ] T represents the reference control variable, [v, ω] T represents the speed and angular velocity that the vehicle needs to reach when driving to the target path point.

[0094] Step 413, based on the vehicle kinematics equation, the error state variable and the error control variable, a linear error equation is obtained. For example, the nonlinear vehicle kinematics equation is converted into a linear time-varying model, the Taylor expansion is performed on the vehicle kinematics equation, and the linear error equation is obtained: wherein, represents the first derivative of the error state variable,

[0095] The process of constructing the cost function based on the linear error equation in step 400 can be:

[0096] The above linear error equation is discretized by using the forward Euler method, and the time step is set to T, and the linear discrete error equation is obtained:

[0097]

[0098] wherein, n is the discrete time index, X e (n+1) represents the error state variable at the n+1th step, X e (n) represents the error state variable at the nth step, u e (n) represents the error control variable at the nth step.

[0099] On the basis of the linear discrete error equation, the cost function J constructed is expressed as:

[0100]

[0101] wherein, N represents the iteration range, Q represents the state error weight matrix, and R represents the control variable weight matrix.

[0102] The hybrid solving mechanism in step 400 includes querying the hash table and iterating the cost function. The process of determining the LQR feedback gain matrix in combination with the cost function using the hybrid solving mechanism includes:

[0103] In step 421, the heading angle index threshold and the speed index threshold are set.

[0104] In step 422, the vehicle state in which the heading angle of the target path point does not exceed the heading angle index threshold and the vehicle expected speed does not exceed the speed index threshold is set as the typical working condition.

[0105] In step 423, the feedback matrix of the typical working condition is determined, and the hash table is constructed.

[0106] In step 424, when the heading angle of the target path point does not exceed the heading angle index threshold and the vehicle expected speed does not exceed the speed index threshold, the feedback matrix is directly queried in the hash table as the LQR feedback gain matrix. For example, the constructed hash table is two-dimensional, the index dimension of the hash table includes the vehicle expected speed and the heading angle of the target path point, and the corresponding feedback matrix is queried in the hash table according to the vehicle expected speed and the heading angle of the target path point as the LQR feedback gain matrix.

[0107] In step 425, when the heading angle of the target path point exceeds the heading angle index threshold or the vehicle expected speed exceeds the speed index threshold, the Riccati equation is obtained according to the cost function and the linear error equation, and the iteration range is set. For example, when the heading angle of the target path point exceeds the heading angle index threshold (i.e. θ ref >θ table ) or the vehicle expected speed exceeds the speed index threshold (i.e. v des >v table ), the Riccati equation is obtained according to the cost function and the linear error equation. Wherein, θ table represents the heading angle index threshold, and v table represents the speed index threshold.

[0108] The obtained Riccati equation is represented as:

[0109] P n =Q+A T P n+1 A-A T P n+1 B(R+B T P n+1 B) -1 B T P n+1 A。

[0110] Step 426, according to the iteration range, iteratively update the Riccati equation. Determine the LQR feedback gain matrix according to the updated Riccati equation. For example, the iteration range is N, initialize the matrix P N = Q, iteratively update the Riccati equation according to the discrete time index n until the Riccati equation converges or the iteration is completed, n = N, N-1,..., 1. In this embodiment, P n represents the matrix updated to n. Calculate the LQR feedback gain matrix according to the matrix of the updated Riccati equation. At time t (i.e. the current time), the LQR feedback gain matrix is represented as: K t = -(R + B T P t+1 B) -1 B T P t A. In the formula, K t represents the LQR feedback gain matrix at time t, t = 0,..., N-1, P t+1 represents the matrix P n in the updated Riccati equation (i.e. at t = 0, take P1 when n = 1).

[0111] As an optional implementation, when the vehicle is driving on a structured road (i.e. a flat road), in order to improve the calculation efficiency of the control quantity, the process of obtaining the LQR control instruction based on the LQR feedback gain matrix in step 500 includes: determining the current error state quantity based on the target path point and the current state information of the vehicle. Obtain the LQR control instruction based on the current error state quantity and the LQR feedback gain matrix.

[0112] In this embodiment, the LQR control instruction can be obtained based on the LQR feedback gain matrix, and the LQR control instruction is taken as the final control instruction. For example, the LQR feedback gain matrix K t obtained according to the above steps is directly used to obtain the final control instruction In the formula, X t represents the error state quantity of the vehicle at time t (i.e. the current time), which is calculated based on the target path point and the current state information of the vehicle (i.e. the position and heading angle of the vehicle at time t) in combination with the formula in step 412 in the above embodiment.

[0113] As an optional implementation, when the vehicle is driving on an unstructured road (e.g. rugged terrain), in order to smoothly control the vehicle and improve the effect of path tracking, the process of obtaining the LQR control instruction based on the LQR feedback gain matrix in step 500 includes: predicting the future state information of the vehicle based on the current state information of the vehicle and the linear error equation. Obtain the LQR control instruction based on the future state information of the vehicle and the LQR feedback gain matrix.

[0114] Further, the final control command is obtained according to the obtained LQR control command. For example, for unstructured roads (such as rugged terrain), if the final control command is only based on the current state without considering the future state change in advance, it may cause a lag effect and reduce the tracking effect. Therefore, a state prediction process can be added, and when calculating the LQR control command, not only the error state quantity X t at the current time is considered, but also the state in a short period of time in the future is predicted, and the control input u t is optimized.

[0115] According to the discrete error state model obtained based on the linear error equation in the above embodiment, the future state X pred of the vehicle (i.e., the future state information of the vehicle) is forward predicted: X pred = AX t + Bu prev . In the formula, X t is the error state quantity at the current time, which is calculated based on the target path point and the current state information of the vehicle (i.e., the position and heading angle of the vehicle at time t), combined with the formula in step 412 in the above embodiment. u prev is the control command at the last time.

[0116] The LQR control command is calculated based on the future state information of the vehicle and the LQR feedback gain matrix:

[0117] u LQR = -K t X pred .

[0118] So that the LQR control command u LQR not only depends on the current state of the vehicle, but also considers the future evolution trend of the vehicle, thereby improving the response speed and trajectory tracking accuracy.

[0119] A first-order low-pass filter is used to smooth the LQR control command to obtain the final control command:

[0120] u out (t) = a u out (t-1) + (1-a) u LQR (t).

[0121] In the formula, u out (t) represents the smoothed actual control output at time t (i.e., the current time), i.e., u out (t) is the final control command; a is a filter coefficient for suppressing high-frequency jitter of the actuator, a ∈ (0, 1); u LQR (t) is the LQR control command at time t (i.e., the current time).

[0122] In an example embodiment, a computer device is provided, which can be a server or a terminal, and an internal structure diagram thereof can be as shown in Figure 2 The computer device includes a processor, a memory, an input / output interface (I / O) and a communication interface. The processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for running the operating system and the computer program in the non-volatile storage medium. The database of the computer device is configured to store unstructured road path tracking and speed optimization control method related data. The input / output interface of the computer device is configured to exchange information between the processor and external devices. The communication interface of the computer device is configured to communicate with external terminals through network connection. The computer program is executed by the processor to implement an unstructured road path tracking and speed optimization control method.

[0123] Those skilled in the art can understand that Figure 2 the structure shown in the above description is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied. The specific computer device can include more or fewer components than those shown in the diagram, or combine certain components, or have a different arrangement of components. In an example embodiment, a computer device is provided, which includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps in the above method embodiments.

[0124] In an example embodiment, a computer readable storage medium is provided, which stores a computer program. The computer program is executed by a processor to implement the steps in the above method embodiments.

[0125] In an example embodiment, a computer program product is provided, which includes a computer program. The computer program is executed by a processor to implement the steps in the above method embodiments.

[0126] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in the present application are all information and data authorized by the user or authorized by all parties, and the collection, use and processing of related data need to comply with relevant regulations.

[0127] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer readable storage medium, and when the computer program is executed, the processes of the above-mentioned embodiments of the methods can be included. Any reference to memory, databases or other media used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical storage, high-density embedded non-volatile memory, resistive memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration but not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.

[0128] The database involved in the embodiments provided in the present application can include at least one of a relational database and a non-relational database. The non-relational database can include a distributed database based on a blockchain, etc., without being limited thereto. The processor involved in the embodiments provided in the present application can be a general-purpose processor, a central processing unit, a graphics processing unit, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., without being limited thereto.

[0129] The technical features of the above embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above embodiments are not described, but as long as the combinations of the technical features do not exist, they should be considered as the scope of the present application.

[0130] The principles and implementation modes of the present application are described by applying specific examples herein. The above description of the embodiments is only used to help understand the method and its core idea of the present application; meanwhile, for those skilled in the art, according to the idea of the present application, the specific implementation mode and application range can be changed. In conclusion, the content of the present application should not be understood as a limitation.

Claims

1. A non-structured road path tracking and speed optimization control method, characterized by, The method comprises the following steps: acquiring current state information of a vehicle and a plurality of path points from a planned path of the vehicle to be driven on an unstructured road; smoothing the path points to obtain smoothed path points; selecting one path point from the smoothed path points as a target path point, and selecting a plurality of continuous path points containing the target path point from the smoothed path points to generate a preview interval and determine a curvature of the preview interval; obtaining a reference control amount based on the current state information of the vehicle and the curvature of the preview interval; acquiring state information of the plurality of continuous path points, and determining a reference state amount of the target path point based on the state information of the plurality of continuous path points; constructing a linear error equation based on the current state information of the vehicle, the reference state amount of the target path point, and the reference control amount; constructing a cost function based on the linear error equation; determining an LQR feedback gain matrix by combining the cost function through a hybrid solving mechanism; obtaining an LQR control instruction based on the LQR feedback gain matrix; obtaining a final control instruction based on the LQR control instruction to control the vehicle to reach the target path point.

2. The non-structured road path following and speed optimization control method of claim 1, wherein, The method of obtaining a reference control amount based on current state information of a vehicle and a curvature of a preview interval comprises the following steps: determining a desired speed of the vehicle based on the current state information of the vehicle and the curvature of the preview interval by using a progressive start-stop strategy; the progressive start-stop strategy comprises a vehicle start speed function and a vehicle stop speed function; obtaining the reference control amount based on the desired speed of the vehicle.

3. The non-structured road path following and speed optimization control method of claim 2, wherein, The method of determining a desired speed of a vehicle based on current state information of the vehicle and a curvature of a preview interval by using a progressive start-stop strategy comprises the following steps: determining a fused curvature based on the current state information of the vehicle and the curvature of the preview interval; obtaining a speed of the vehicle in a start or stop phase based on the current state information of the vehicle by using the progressive start-stop strategy; determining the desired speed of the vehicle based on the current state information of the vehicle, the fused curvature, and the speed of the vehicle in the start or stop phase.

4. The non-structured road path following and speed optimization control method of claim 1, wherein, The method of constructing a linear error equation based on current state information of a vehicle, a reference state amount of a target path point, and a reference control amount comprises the following steps: constructing a vehicle kinematics equation based on the current state information of the vehicle; obtaining an error state amount and an error control amount based on the current state information of the vehicle, the reference state amount of the target path point, and the reference control amount; obtaining the linear error equation based on the vehicle kinematics equation, the error state amount, and the error control amount.

5. The non-structured road path following and speed optimization control method of claim 2, wherein, The hybrid solving mechanism comprises a query hash table and an iterative optimization cost function; The method of determining an LQR feedback gain matrix by combining a cost function through a hybrid solving mechanism comprises the following steps: setting a heading angle index threshold and a speed index threshold; regarding a vehicle state in which a heading angle of the target path point does not exceed the heading angle index threshold and a desired speed of the vehicle does not exceed the speed index threshold as a typical working condition; determining a feedback matrix of the typical working condition to construct a hash table; when the heading angle of the target path point does not exceed the heading angle index threshold and the vehicle desired speed does not exceed the speed index threshold, directly querying a feedback matrix in the hash table as the LQR feedback gain matrix; when the heading angle of the target path point exceeds the heading angle index threshold or the vehicle desired speed exceeds the speed index threshold, obtaining a Riccati equation according to a cost function and the linear error equation, and setting an iteration range; iteratively updating the Riccati equation according to the iteration range; determining the LQR feedback gain matrix according to the updated Riccati equation.

6. The non-structured road path following and speed optimization control method of claim 1, wherein, obtaining an LQR control instruction based on the LQR feedback gain matrix, including: determining a current error state quantity based on the target path point and the vehicle current state information; obtaining an LQR control instruction based on the current error state quantity and the LQR feedback gain matrix.

7. The unstructured road path following and speed optimization control method of claim 1, wherein, obtaining an LQR control instruction based on the LQR feedback gain matrix, including: predicting vehicle future state information based on the vehicle current state information and the linear error equation; obtaining an LQR control instruction based on the vehicle future state information and the LQR feedback gain matrix.

8. A computer device comprising: A memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that the processor executes the computer program to implement the non-structured road path tracking and speed optimization control method of any one of claims 1-7.

9. A computer readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the non-structured road path tracking and speed optimization control method of any one of claims 1-7.

10. A computer program product comprising a computer program, characterized in that, The computer program is executed by the processor to implement the non-structured road path tracking and speed optimization control method of any one of claims 1-7.

Images