A trajectory tracking method and system for an unmanned test vehicle

Abstract

The application discloses a kind of unmanned test vehicle trajectory tracking method and system, comprising: trajectory tracking prediction model is constructed based on test vehicle kinematics model, and according to the error range between set actual trajectory and preset trajectory, design proportion factor rule base;Actual trajectory is obtained, and the error amount between actual trajectory and preset trajectory is determined, and according to error amount in proportion factor rule base, corresponding proportion factor is selected, the stability and accuracy of model are adjusted in real time, to update trajectory tracking prediction model;Using the updated trajectory tracking prediction model, according to the number of prediction period, error amount is divided into single prediction period, error amount in single prediction period is converted into control amount, according to control amount control wheel speed, correct the error between actual trajectory and preset trajectory, to track preset trajectory.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of vehicle control, in particular to a trajectory tracking method and system for unmanned test vehicle. BACKGROUND

[0002] The statements in this section merely provide background information related to the present application and do not necessarily constitute the prior art.

[0003] The tool vehicle required for testing unmanned vehicle is generally a manually driven vehicle or a simulation car driving along a specific trajectory. Trajectory control generally combines the latitude, longitude and yaw angle data fed back by the Beidou satellite navigation system (BDS) and the inertial navigation system (INS) to control and track the preset trajectory, and controls in real time by calculating the vehicle speed and wheel rotation angle at each time, so as to realize trajectory tracking of the vehicle. Common trajectory tracking control algorithms include PID, linear quadratic regulator (LQR) and pure pursuit control algorithm (PurePursuit).

[0004] However, the common trajectory tracking algorithms have certain limitations. The PID algorithm only focuses on the error at the current time, and when the chassis vehicle speed increases, overshoot is prone to occur, which is suitable for low-speed trajectory tracking and small path curvature; the LQR algorithm does not consider the error caused by actual disturbance to the tracking system, and the tracking effect is not good for trajectories with too large curvature; the PurePursuit algorithm is suitable for low-speed trajectory tracking, and when the vehicle speed is large, the turning inscribed phenomenon is prone to occur. SUMMARY

[0005] In order to solve the above problems, the present application provides a trajectory tracking method and system for unmanned test vehicle, which updates the trajectory tracking prediction model according to the error between the actual trajectory and the preset trajectory, corrects the error between the actual trajectory and the preset trajectory, and adjusts the stability and accuracy of the model in real time.

[0006] In order to achieve the above purpose, the present application adopts the following technical scheme:

[0007] In a first aspect, the present application provides a trajectory tracking method for unmanned test vehicle, comprising:

[0008] constructing a trajectory tracking prediction model based on the kinematic model of the test vehicle, and designing a proportional factor rule base according to the error range between the actual trajectory and the preset trajectory;

[0009] acquiring the actual trajectory, determining the error between the actual trajectory and the preset trajectory, selecting the corresponding proportional factor in the proportional factor rule base according to the error, and updating the trajectory tracking prediction model accordingly;

[0010] The updated trajectory tracking prediction model is used to divide the error into a single prediction period according to the number of prediction periods, convert the error in a single prediction period into a control amount, and control the wheel speed according to the control amount, so as to track the preset trajectory.

[0011] As an alternative embodiment, the target optimization function of the trajectory tracking prediction model is designed as:

[0012]

[0013] Wherein, J(K) is a performance index; α is an error weight, and β is a control weight; is the prediction trajectory output at k+i at k, and yef(k+i|k) is the preset trajectory output at k+i at k, is the control amount increment of k+i calculated at k, H Pro is the interval of the prediction trajectory divided at the sampling period.

[0014] As an alternative embodiment, the error includes distance error and heading angle error.

[0015] As an alternative embodiment, the scale factor is the ratio of the error weight and the control weight.

[0016] As an alternative embodiment, the construction process of the scale factor rule base is: setting the error range of the distance error and the heading angle error, dividing the error range of the two into a plurality of sets, setting the output range of the scale factor, dividing the output range into a plurality of sets, combining the distance error set and the heading angle error set, and different combinations correspond to different output range sets.

[0017] As an alternative embodiment, when the wheel speed is controlled according to the control amount, the wheel speed is controlled according to the control amount of the first prediction period, and the control amounts of other prediction periods are discarded.

[0018] As an alternative embodiment, when the wheel speed is controlled according to the control amount, the control amount is converted into a pulse frequency to control the speed of the left and right wheels.

[0019] As an alternative embodiment, the construction process of the test vehicle kinematics model includes: in a plane coordinate system, constructing the test vehicle kinematics model according to the speed of the left and right wheels of the test vehicle at a certain time and the angle between the vehicle speed direction and the X axis.

[0020] In a second aspect, the present application provides a trajectory tracking system for an unmanned test vehicle, comprising:

[0021] The model construction module is configured to construct a trajectory tracking prediction model based on a test vehicle kinematics model, and design a proportional factor rule base according to an error range between a preset trajectory and an actual trajectory;

[0022] The update module is configured to acquire the actual trajectory, determine an error amount between the actual trajectory and the preset trajectory, select a corresponding proportional factor in the proportional factor rule base according to the error amount, and update the trajectory tracking prediction model;

[0023] The control module is configured to adopt the updated trajectory tracking prediction model, divide the error amount into a single prediction period according to a prediction period number, convert the error amount in the single prediction period into a control amount, and control a wheel speed according to the control amount, so as to track the preset trajectory.

[0024] In a third aspect, the present application provides an electronic device, comprising a memory and a processor, and computer instructions stored in the memory and running on the processor, when the computer instructions are run by the processor, the method of the first aspect is completed.

[0025] In a fourth aspect, the present application provides a computer readable storage medium for storing computer instructions, when the computer instructions are executed by a processor, the method of the first aspect is completed.

[0026] Compared with the prior art, the present application has the following beneficial effects:

[0027] The present application provides a trajectory tracking method and system for an unmanned test vehicle, a trajectory tracking prediction model is constructed based on a test vehicle kinematics model, an error amount between an actual trajectory and a preset trajectory is divided into each prediction period according to a prediction period number and a remaining amount in a single prediction period is converted into a control amount, the test vehicle is acted on by a pulse frequency, the preset trajectory is tracked, the error between the actual trajectory and the preset trajectory can be corrected online, the stability and accuracy of the system can be adjusted in real time, the trajectory tracking prediction model is updated, and the tracking stability and tracking accuracy are better. Compared with common control algorithms, the robustness is better when there is interference in the outside world, and the linear and curved trajectory tracking accuracy and stability are higher.

[0028] The present application adjusts the model stability and accuracy parameters in each sampling period, when the actual trajectory and the preset trajectory error is larger, the prediction model focuses more on the trajectory tracking accuracy and focuses on the rapid tracking of the trajectory, when the actual trajectory and the preset trajectory error is smaller, the prediction model focuses more on the stability of the trajectory tracking, and prevents the controlled vehicle from appearing overshoot when approaching the preset trajectory, which causes the problem of trajectory tracking fluctuation.

[0029] Advantages of the additional aspects of the application will become apparent in the following description. BRIEF DESCRIPTION OF DRAWINGS

[0030] The accompanying drawings, which constitute a part of this specification, are included to provide a further understanding of the application, and are incorporated in and constitute a part of this specification. The embodiments of the application, together with their

[0031] Figure 1 A schematic diagram of a trajectory tracking method architecture for an unmanned test vehicle of embodiment 1 of the application is provided;

[0032] Figure 2 A schematic diagram of equipment used for a test vehicle of embodiment 1 of the application is provided;

[0033] Figure 3 A schematic diagram of a kinematic model for a test vehicle of embodiment 1 of the application is provided;

[0034] Figure 4 A schematic diagram of a scaling factor rule base for embodiment 1 of the application is provided;

[0035] Figure 5 A schematic diagram of a straight line tracking effect for embodiment 1 of the application is provided;

[0036] Figure 6 A schematic diagram of a curve tracking effect for embodiment 1 of the application is provided. DETAILED DESCRIPTION

[0037] The application will be further described with reference to the drawings and embodiments.

[0038] It should be noted that the following detailed description is exemplary in nature and is intended to provide further description of the application. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0039] It is also to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting, as the scope of the application will be limited only by the appended claims. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. Unless otherwise required by context, singular terms shall include pluralities and vice versa. Plural elements can be separated by a hyphenated form of the element name, for example, "multiple components", unless otherwise indicated. Unless otherwise indicated, the use of "or" in the

[0040] In the case of no conflict, the embodiments in the present application and the features in the embodiments can be combined with each other.

[0041] Embodiment 1

[0042] According to the analysis of the current common vehicle trajectory tracking algorithm, although the PID type, LQR, PurePursuit and other control algorithms can be applied to trajectory tracking control, there are problems of poor tracking effect, which may affect the accuracy of the unmanned vehicle test. In view of this, the present embodiment proposes a trajectory tracking method for unmanned test vehicle, which can correct the error between the actual trajectory and the preset trajectory online, has strong robustness to external interference, adjusts the stability and tracking accuracy of the prediction model in real time, has better tracking stability and higher tracking accuracy.

[0043] As shown in Figure 1 , the method specifically comprises:

[0044] A trajectory tracking prediction model is constructed based on the kinematic model of the test vehicle, and a proportional factor rule base is designed according to the error range between the actual trajectory and the preset trajectory;

[0045] In each sampling period, the actual trajectory at the current time is obtained, and the distance error and the heading angle error between the actual trajectory and the preset trajectory are determined, and the corresponding proportional factor is selected in the proportional factor rule base according to the error, so as to adjust the stability parameter and the accuracy parameter of the trajectory tracking prediction model, thereby updating the trajectory tracking prediction model;

[0046] The updated trajectory tracking prediction model is used to divide the error into a single prediction period according to the number of prediction periods, convert the error in a single prediction period into a control amount, and control the wheel speed according to the control amount, so as to track the preset trajectory;

[0047] Here, because of the interference of the outside to the test vehicle, it cannot be ensured that each sampling period completes the specified target value according to the output control amount, so only the control amount of the first prediction period is output, and the control amount of other prediction periods is discarded.

[0048] In the present embodiment, as shown in Figure 2 , the test vehicle is composed of a rechargeable lithium battery, a servo motor, a servo motor controller, a BDS, a Linux hardware terminal and the like; wherein the rechargeable lithium battery provides a 24V voltage, the BDS and the INS device feedback the current position and the yaw angle of the vehicle through the serial port, the Linux hardware terminal runs the trajectory tracking method, converts the control amount into a pulse frequency, and outputs it to the servo motor controller through the IO interface, and the servo motor controller controls the speed of the left and right wheels to control the driving state.

[0049] In the present embodiment, as shown inFigure 3 The test vehicle kinematics model is shown, the motion state of the vehicle at a certain time is analyzed under the XY plane coordinate system, the kinematics model is established according to the angle between the speed direction of the vehicle and the X axis and the speed of the left and right wheels at the time, and each reference point on the reference trajectory satisfies the kinematics equation:

[0050]

[0051] Where, V R is the right wheel speed, V L is the left wheel speed, is the angle between the speed direction of the vehicle and the X axis, and W is the center distance of the left and right wheels.

[0052] Let the input vector U = [V R , V L ], the state vector The kinematics model can be regarded as a control system composed of input vector U and state vector X, the kinematics model is linearized by using Taylor series expansion and ignoring high order terms, and finally discrete processing is carried out.

[0053] In this embodiment, the trajectory tracking prediction model is constructed based on the test vehicle kinematics model:

[0054] Because there are multiple input variables and output variables, the state space model is established:

[0055]

[0056] Where, M is the state matrix, N is the input matrix, Q is the output matrix, and Y is the output vector.

[0057] The extended state vector ζ including the state vector X and the input vector U is defined:

[0058]

[0059] Where, X(k) is the chassis car position and yaw angle at time k, and U(k-1) is the chassis car control amount at time k-1.

[0060] The state space model is obtained by:

[0061]

[0062] Where, I is the unit matrix, and ΔU is the control amount increment.

[0063] The target optimization function is designed:

[0064]

[0065] Wherein, J(K) is the performance index; the first term represents the ability to track the preset trajectory, and the second term represents the ability to track the system stability; a is the error weight, which represents the accuracy parameter of the trajectory tracking prediction model; β is the control weight, which represents the stability parameter of the trajectory tracking prediction model, and the error weight and the control weight can be adjusted according to the real-time feedback trajectory data; is the predicted trajectory output quantity at k+i moment, yef(k+i|k) is the preset trajectory output quantity at k+i moment, is the control quantity increment at k+i moment calculated at k moment, H Pro is the interval divided by the prediction model for the predicted trajectory in a sampling period.

[0066] For convenience of solving, the problem of obtaining the optimal control quantity of the target optimization function is converted into a quadratic programming problem, and the constraint conditions of the left and right side wheel speeds and the speed increments are set; the left and right side wheel speed increments are solved by quadratic programming, and the speed increments of the previous sampling period are accumulated and output at each sampling period.

[0067] In the embodiment, the ratio of the accuracy parameter and the stability parameter of the trajectory tracking prediction model is taken as the proportion factor, a proportion factor rule base based on the distance error and the heading angle error between the preset trajectory and the actual trajectory is set, at each sampling period, the distance error and the heading angle error between the chassis vehicle preset trajectory and the actual trajectory are calculated, the appropriate proportion factor is selected in the rule base according to the error, and the stability parameter and the accuracy parameter of the trajectory tracking system are adjusted.

[0068] The value of the proportion factor μ is determined according to the distance error and the heading angle error, and the proportion factor rule base is specifically designed as follows:

[0069] (1) The distance error and the heading angle error are taken as inputs, and the proportion factor is taken as output, and the Gaussian membership function is selected for the input and output;

[0070] (2) The error range of the distance error and the heading angle error is set, the error range of the two is divided into several sets, the output range of the proportion factor is set, the output range is divided into several sets, the distance error set and the heading angle error set are combined, and different combinations correspond to different output range sets; as shown in the following table: Figure 4

[0071] (3) According to the set combination of the distance error and the heading angle error between the actual trajectory and the preset trajectory, the proportion factor is selected through the proportion factor rule base.

[0072] ​(4) In the updating, the trajectory tracking prediction model calculates the left and right wheel speed control amount in one sampling period, and tracks the trajectory with the control amount; in the next sampling period, the actual trajectory of the vehicle is fed back to the prediction model, the stability and accuracy parameters are adjusted again, the model parameters are updated, and the control amount is calculated, and the above is repeated.

[0073] In the embodiment, the updated trajectory tracking prediction model is used to divide the distance error and the heading angle error into each prediction period according to the number of prediction periods, wherein the amount of distance error and heading angle error that each prediction period should be divided is calculated according to the target optimization function constructed above, the distance error or the heading angle error in each prediction period is not equal, and the sum of the distance error or the heading angle error in each prediction period is equal to the total distance error or the total heading angle error.

[0074] Then, the error amount in a single prediction period is converted into a control amount, the control amount is converted into a pulse frequency, and the pulse frequency is applied to the test vehicle to track the preset trajectory by controlling the rotation speed of the left and right wheels, such as Figure 5 is the tracking effect of a straight line trajectory (t is time); Figure 6 is the tracking effect of a curve trajectory (t is time).

[0075] Embodiment 2

[0076] The embodiment provides an unmanned test vehicle trajectory tracking system, comprising:

[0077] A model construction module configured to construct a trajectory tracking prediction model based on a test vehicle kinematics model, and design a proportional factor rule base according to an error range between a preset trajectory and an actual trajectory;

[0078] An updating module configured to obtain the actual trajectory, determine the error amount between the actual trajectory and the preset trajectory, select a corresponding proportional factor in the proportional factor rule base according to the error amount, and update the trajectory tracking prediction model;

[0079] A control module configured to use the updated trajectory tracking prediction model, divide the error amount into a single prediction period according to the number of prediction periods, convert the error amount in a single prediction period into a control amount, and control the rotation speed of the wheels according to the control amount, so as to track the preset trajectory.

[0080] It should be noted that the above modules correspond to the steps described in Embodiment 1, and the above modules have the same examples and application scenarios as the corresponding steps, but are not limited to the content disclosed in Embodiment 1. It should be noted that the above modules as part of the system can be executed in a computer system such as a group of computer executable instructions.

[0081] In more embodiments, there is also provided:

[0082] An electronic device comprising a memory and a processor and computer instructions stored on the memory and running on the processor, when the computer instructions are run by the processor, the method described in embodiment 1 is completed. For brevity, it will not be described here.

[0083] It should be understood that in the embodiments, the processor can be a central processing unit CPU, and the processor can also be other general-purpose processors, digital signal processors DSP, application-specific integrated circuits ASIC, ready programmable gate arrays FPGA or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc.

[0084] The memory can include read-only memory and random access memory, and provide instructions and data to the processor, and a portion of the memory can also include non-volatile random access memory. For example, the memory can also store device type information.

[0085] A computer readable storage medium for storing computer instructions, when the computer instructions are executed by the processor, the method described in embodiment 1 is completed.

[0086] The method in embodiment 1 can be directly embodied as hardware processor execution is completed, or executed by a combination of hardware and software modules in the processor. The software module can be located in a storage medium mature in the art such as random access memory, flash memory, read-only memory, programmable read-only memory or electrically erasable programmable memory, register, etc. The storage medium is located in the memory, and the processor reads the information in the memory, and combines the hardware to complete the steps of the above method. To avoid repetition, it will not be described in detail here.

[0087] Those of ordinary skill in the art can realize that the units of the examples described in combination with the embodiments, i.e. the algorithm steps, can be realized in electronic hardware or in a combination of computer software and electronic hardware. Whether the functions are executed in hardware or software mode depends on the specific application and design constraints of the technical solution. The skilled person can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present application.

[0088] The above describes the specific embodiments of the application in combination with the drawings, but is not a limitation on the protection scope of the application. Those skilled in the art should understand that various modifications or variations made by those skilled in the art on the basis of the technical solutions of the application without inventive labor are still within the protection scope of the application.

Claims

1. A method for tracking the trajectory of an unmanned test vehicle, characterized in that, include: A trajectory tracking prediction model is constructed based on the kinematic model of the test vehicle, and a scaling factor rule library is designed according to the error range between the actual trajectory and the preset trajectory. The construction process of the scaling factor rule base is as follows: with distance error and heading angle error as inputs and scaling factor as output, both inputs and outputs use Gaussian membership functions; set the error ranges of distance error and heading angle error, divide the two error ranges into several sets, set the output range of scaling factor, divide the output range into several sets, and combine the distance error set and the heading angle error set. Different combinations correspond to different output range sets. The actual trajectory is obtained, and the error between the actual trajectory and the preset trajectory is determined. The corresponding scaling factor is selected from the scaling factor rule base according to the error, thereby updating the trajectory tracking prediction model. The updated trajectory tracking prediction model is adopted. Based on the number of prediction cycles, the error is divided into a single prediction cycle. The error in a single prediction cycle is converted into a control variable. The wheel speed is controlled according to the control variable to track the preset trajectory. When controlling wheel speed based on control quantity, wheel speed is controlled based on control quantity of the first prediction cycle, and control quantities of other prediction cycles are discarded. When controlling the wheel speed based on the control quantity, the control quantity is converted into a pulse frequency to control the speed of the left and right wheels.

2. The method for tracking the trajectory of an unmanned test vehicle as described in claim 1, characterized in that, The objective optimization function of the trajectory tracking prediction model is designed as follows: in, These are performance metrics; α is the error weight, and β is the control weight. It is the predicted trajectory output at time k for time k+i. It is the output of the preset trajectory at time k+i. It is the increment of the control quantity at time k+i calculated at time k. It refers to the intervals into which the predicted trajectory is divided during the sampling period.

3. The method for tracking the trajectory of an unmanned test vehicle as described in claim 2, characterized in that, The error includes distance error and heading angle error, and the scaling factor is the ratio of error weight to control weight.

4. The method for tracking the trajectory of an unmanned test vehicle as described in claim 1, characterized in that, The process of constructing the kinematic model of the test vehicle includes: constructing the kinematic model of the test vehicle in a planar coordinate system based on the velocities of the left and right wheels of the test vehicle at a certain moment and the angle between the vehicle's velocity direction and the X-axis.

5. A trajectory tracking system for an unmanned test vehicle, characterized in that, include: The model building module is configured to build a trajectory tracking prediction model based on the kinematic model of the test vehicle, and design a scaling factor rule library according to the error range between the actual trajectory and the preset trajectory. The construction process of the scaling factor rule base is as follows: with distance error and heading angle error as inputs and scaling factor as output, both inputs and outputs use Gaussian membership functions; set the error ranges of distance error and heading angle error, divide the two error ranges into several sets, set the output range of scaling factor, divide the output range into several sets, and combine the distance error set and the heading angle error set. Different combinations correspond to different output range sets. The update module is configured to acquire the actual trajectory, determine the error between the actual trajectory and the preset trajectory, select the corresponding scaling factor in the scaling factor rule base according to the error, and update the trajectory tracking prediction model accordingly. The control module is configured to use an updated trajectory tracking prediction model, divide the error into a single prediction cycle according to the number of prediction cycles, convert the error in a single prediction cycle into a control quantity, and control the wheel speed according to the control quantity to track the preset trajectory. When controlling wheel speed based on control quantity, wheel speed is controlled based on control quantity of the first prediction cycle, and control quantities of other prediction cycles are discarded. When controlling the wheel speed based on the control quantity, the control quantity is converted into a pulse frequency to control the speed of the left and right wheels.

6. An electronic device, characterized in that, It includes a memory and a processor, as well as computer instructions stored in the memory and running on the processor, which, when executed by the processor, perform the method according to any one of claims 1-4.

7. A computer-readable storage medium, characterized in that, Used to store computer instructions, which, when executed by a processor, perform the method described in any one of claims 1-4.

Images