Path tracking control method and device for tracked vehicle and storage medium

Abstract

The invention provides a path tracking control method and device for a tracked vehicle and a storage medium. The method comprises the following steps: acquiring an expected path of a tracked vehicle, a current position of the tracked vehicle, a state of the tracked vehicle and a position of an obstacle; constructing a vehicle dynamics model at least according to the current position; constructing a safe driving corridor at least according to the expected path, the current position and the position of the obstacle; under the condition that the current position is not located in the safe driving corridor, a dynamic window search control algorithm is adopted to determine an optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path; and under the condition that the current position is located in the safe driving corridor, the driving torque is calculated by adopting a following path constraint algorithm, and the tracked vehicle is controlled to track the expected path according to the driving torque. The problem that in the prior art, the safety and accuracy of path tracking control of the tracked vehicle are not high is solved.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving, and more specifically, to a path tracking control method for a tracked vehicle, a path tracking control device for a tracked vehicle, a computer-readable storage medium, and a computer program product. Background Art

[0002] Path tracking control is a key technology for unmanned tracked vehicles. It achieves autonomous tracking by controlling the speed of the left and right track motors of the vehicle to follow the desired obstacle avoidance path. Existing path tracking control methods focus on tracking accuracy. Tracking control overshoot is prone to occur in complex driving environments. The error between the actual driving path and the desired path is difficult to effectively control, affecting the safety and stability of the unmanned tracked vehicle path tracking. Therefore, how to design a path tracking algorithm that takes into account the kinematic and dynamic characteristics of tracked vehicles, has low complexity, and takes into account both tracking safety and accuracy is of great significance to the implementation of unmanned tracked vehicles in actual application scenarios. Summary of the invention

[0003] The main purpose of the present application is to provide a path tracking control method for a tracked vehicle, a path tracking control device for a tracked vehicle, a computer-readable storage medium and a computer program product, so as to at least solve the problem of low safety and accuracy of path tracking control of tracked vehicles in the prior art.

[0004] According to one aspect of the present application, a path tracking control method for a tracked vehicle is provided, comprising: obtaining a desired path of the tracked vehicle, a current position of the tracked vehicle, a state of the tracked vehicle, and a position of an obstacle, wherein the desired path is a preset driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; constructing a vehicle dynamics model at least according to the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; constructing a safe driving corridor at least according to the desired path, the current position, and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with the obstacle during path tracking; determining whether the current position is within the safe driving corridor, and if the current position is not within the safe driving corridor, determining an optimal path using a dynamic window search control algorithm, and controlling the tracked vehicle to enter the safe driving corridor according to the optimal path; and calculating a driving torque using a following path constraint algorithm when the current position is within the safe driving corridor, and controlling the tracked vehicle to track the desired path according to the driving torque.

[0005] Optionally, obtaining the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle includes: obtaining a plurality of target points, the plurality of target points are sequentially connected to form the expected path; according to the formula Determine the current position P veh , where x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system, is the heading angle of the tracked vehicle; obtain the center of mass linear velocity of the tracked vehicle, the angular velocity of the tracked vehicle, the rotation angle of the left track of the tracked vehicle, the linear velocity of the left track of the tracked vehicle, the rotation angle of the right track of the tracked vehicle and the linear velocity of the right track of the tracked vehicle to obtain the state of the tracked vehicle; according to the formula: Determine the position P of the obstacle obstacle , where x obs and obs is the position coordinate of the obstacle in the geodetic coordinate system, is the heading angle of the obstacle.

[0006] Optionally, constructing a vehicle dynamics model based at least on the current position includes: The Lagrangian equation L of the tracked vehicle is constructed, wherein m is the mass of the tracked vehicle, J is the moment of inertia about the center of mass of the tracked vehicle, ξ is the first state vector of the tracked vehicle, M is the inertia matrix of the tracked vehicle; according to the formula Construct the dynamic equation u(t) of the tracked vehicle, where C is the Coriolis centrifugal force of the tracked vehicle; according to the formula Construct a kinematic model of the tracked vehicle, where L is the distance between the left track and the right track of the tracked vehicle, b is the average track width of the tracked vehicle, θ L is the rotation angle of the left track of the tracked vehicle, θ R is the rotation angle of the right track of the tracked vehicle, ζ is the second state vector of the tracked vehicle, ζ=[θ R ,θ L ] T , Substituting the kinematic model into the dynamic equation, the vehicle dynamics model is obtained. in, Wherein, u is the driving torque of the tracked vehicle.

[0007] Optionally, constructing a safe driving corridor based at least on the expected path, the current position and the position of the obstacle comprises: Calculating safe area Among them, D(P i des ,P veh ) is the current position P veh and the i-th target point P i des The Euclidean distance between the target points, the circle with each target point as the center and R as the radius does not contain the obstacle, and the expected path includes multiple target points connected in sequence; according to the formula The safe driving corridor SafeZone is constructed, wherein n is the number of the target points.

[0008] Optionally, after constructing a safe driving corridor based at least on the expected path, the current position and the position of the obstacle, and before determining whether the position of the vehicle is within the safe driving corridor, the method further comprises: determining the safe driving corridor according to formula e safezone =ln[R 2 -D 2 (P i des ,P veh )], calculate the safety constraint e of the tracked vehicle safezone , where D(P i des ,P veh ) is the current position P veh and the i-th target point P i des The Euclidean distance between the target points, the circle with each target point as the center and R as the radius does not contain the obstacle, and the expected path includes multiple target points connected in sequence, x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system, is the heading angle of the tracked vehicle, and is the position coordinate of the i-th target point in the geodetic coordinate system, is the heading angle of the i-th target point; according to the formula Calculate the target constraint e of the tracked vehicle target When the safety constraint and the target constraint approach 0 at the same time, the first state vector ξ of the tracked vehicle satisfies in,

[0009] Optionally, when the current position is not within the safe driving corridor, a dynamic window search control algorithm is used to determine an optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path, including: a determination step, according to V sample ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]}, determine the sampling range V of the tracked vehicle sample , where v is the linear velocity of the center of mass of the tracked vehicle, ω is the angular velocity of the tracked vehicle, and v min is the minimum value of the center of mass linear velocity of the tracked vehicle, v max is the maximum value of the center-of-mass linear velocity of the tracked vehicle, ω min is the minimum angular velocity of the tracked vehicle, ω m a x is the maximum value of the angular velocity of the tracked vehicle; the calculation steps are according to the formula: Calculate the number of samples N samp , where Δv is the sampling speed interval of the center of mass linear velocity of the tracked vehicle, and Δω is the sampling speed interval of the angular velocity of the tracked vehicle; the updating step is based on the formula: For the current position P veh Update to get the updated position P pre ,in, Δt is the sampling time interval of the center of mass linear velocity of the tracked vehicle and the angular velocity of the tracked vehicle; the first repetition step, repeating the updating step multiple times, and updating the current position to the updated position obtained in the last repetition process in the repetition process until the preset number of times is reached, and obtaining N samp Preset trajectory; calculation steps, according to the formula E = g × D (P j ,P obstacle )+h×(π-Δθ), calculate the evaluation function E of each preset trajectory, where g and h are weight coefficients, g and h are both greater than 0, D(P j ,P obstacle ) is the last position P in the preset trajectory j With the obstacle P obstacle The Euclidean distance between the positions, Δθ is P j The tangent direction of i desThe angle of the connecting line of the desired path includes a plurality of the target points connected in sequence; a determination step, determining that the preset trajectory corresponding to the maximum value of the plurality of evaluation functions is the optimal trajectory, and obtaining the linear velocity of the tracked vehicle on the optimal trajectory and the angular velocity of the tracked vehicle on the optimal trajectory; a first control step, according to the formula Calculate the left track speed v of the tracked vehicle L , and controlling the left track of the tracked vehicle to travel at the left track speed, wherein v opt is the linear speed of the tracked vehicle on the optimal trajectory, L is the distance between the left track and the right track of the tracked vehicle, b is the average track width of the tracked vehicle, ω opt is the angular velocity of the tracked vehicle on the optimal trajectory; the second control step, according to the formula Calculate the right track speed v of the tracked vehicle R , and controlling the right track of the tracked vehicle to travel at the right track speed; a second repetitive step, sequentially repeating the determining step, the calculating step, the updating step, the first repetitive step, the calculating step, the determining step, the first control step and the second control step at least once until the tracked vehicle enters the safe driving corridor.

[0010] Optionally, when the current position is within the safe driving corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to track the desired path according to the driving torque, including: according to the formula Calculate the driving torque in, M is the inertia matrix of the tracked vehicle, ζ is the second state vector of the tracked vehicle, ζ=[θ R ,θ L ] T ,θ L is the rotation angle of the left track of the tracked vehicle, θ R is the rotation angle of the right track of the tracked vehicle, C is the Coriolis centrifugal force of the tracked vehicle, and κ is an adjustable coefficient greater than 0.

[0011] According to another aspect of the present application, a path tracking control device for a tracked vehicle is provided, comprising: an acquisition unit, for acquiring a desired path of the tracked vehicle, a current position of the tracked vehicle, a state of the tracked vehicle, and a position of an obstacle, wherein the desired path is a pre-set driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; a first construction unit, for constructing a vehicle dynamics model based at least on the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; and a second construction unit, for constructing a vehicle dynamics model based at least on the desired path, the current position, and the position of the obstacle. A safe driving corridor is constructed, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with the obstacle during path tracking; a first control unit is used to determine whether the current position is located in the safe driving corridor, and when the current position is not located in the safe driving corridor, a dynamic window search control algorithm is used to determine an optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path; a second control unit is used to calculate a driving torque using a following path constraint algorithm when the current position is located in the safe driving corridor, and control the tracked vehicle to track the desired path according to the driving torque.

[0012] According to another aspect of the present application, a computer-readable storage medium is provided, wherein the computer-readable storage medium includes a stored program, wherein when the program is executed, the device where the computer-readable storage medium is located is controlled to execute any one of the methods described.

[0013] According to another aspect of the present application, a computer program product is provided, comprising a computer program, wherein when the computer program is executed by a processor, the steps of any one of the methods described are implemented.

[0014] By applying the technical solution of the present application, first, the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle are obtained, wherein the expected path is a pre-set driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; then, a vehicle dynamics model is constructed at least according to the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; then, a safe driving corridor is constructed at least according to the expected path, the current position and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with obstacles during path tracking; finally, it is determined whether the current position is within the safe driving corridor. If the current position is not within the safe driving corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path; if the current position is within the safe driving corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to track the expected path according to the driving torque. The path tracking control method of the tracked vehicle at least includes three parts: vehicle dynamics model, safe driving corridor and path tracking control algorithm design. The vehicle dynamics model can fully combine the structural characteristics of the tracked vehicle, analyze its mechanical characteristics, and construct a kinematic and dynamic model. The safe driving corridor can take into account the safety of the tracked vehicle during the tracking process. The path tracking control algorithm designs a path tracking control algorithm based on dynamic window search when the initial condition vehicle is outside the safe corridor, and a path tracking control algorithm based on constraint following in the safe corridor. The tracked vehicle is controlled to run so that when the initial condition vehicle is outside the safe corridor, it can avoid obstacles and drive into the safe corridor, and accurately track the expected path in the safe corridor, thereby improving the safety and accuracy of the path tracking control and meeting the needs of actual operation scenarios. The problem of low safety and accuracy of the path tracking control of tracked vehicles in the prior art is solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] The drawings constituting part of the present application are used to provide a further understanding of the present application. The exemplary embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0016] Figure 1 A hardware structure block diagram of a mobile terminal for executing a path tracking control method for a tracked vehicle provided in an embodiment of the present application is shown;

[0017] Figure 2 A schematic flow chart of a path tracking control method for a tracked vehicle provided according to an embodiment of the present application is shown;

[0018] Figure 3A schematic diagram of a process of a safe driving corridor provided according to an embodiment of the present application is shown;

[0019] Figure 4 A specific flow chart of a path tracking control method for a tracked vehicle provided according to an embodiment of the present application is shown;

[0020] Figure 5 A structural block diagram of a path tracking control device for a tracked vehicle provided according to an embodiment of the present application is shown.

[0021] The above drawings include the following reference numerals:

[0022] 102, processor; 104, memory; 106, transmission device; 108, input and output devices. DETAILED DESCRIPTION

[0023] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0024] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present application.

[0025] It should be noted that 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 are not necessarily used to describe a specific order or sequential order. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present application described here. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0026] As introduced in the background technology, the safety and accuracy of the path tracking control of tracked vehicles in the prior art are not high. To solve the above problems, the embodiments of the present application provide a path tracking control method for a tracked vehicle, a path tracking control device for a tracked vehicle, a computer-readable storage medium and a computer program product.

[0027] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present invention.

[0028] The method embodiments provided in the embodiments of the present application can be executed in a mobile terminal, a computer terminal or a similar computing device. Taking running on a mobile terminal as an example, Figure 1 FIG. 1 is a hardware structure block diagram of a mobile terminal of a path tracking control method for a tracked vehicle according to an embodiment of the present invention. Figure 1 As shown, the mobile terminal may include one or more ( Figure 1 Only one is shown in the figure) a processor 102 (the processor 102 may include but is not limited to a processing device such as a microprocessor MCU or a programmable logic device FPGA) and a memory 104 for storing data, wherein the mobile terminal may also include a transmission device 106 and an input / output device 108 for communication functions. It can be understood by those skilled in the art that Figure 1 The structure shown is only for illustration and does not limit the structure of the mobile terminal. Figure 1 More or fewer components as shown, or with Figure 1 Different configurations shown.

[0029] The memory 104 can be used to store computer programs, for example, software programs and modules of application software, such as the computer program corresponding to the path tracking control method of the tracked vehicle in the embodiment of the present invention. The processor 102 executes various functional applications and data processing by running the computer program stored in the memory 104, that is, the above method is implemented. The memory 104 may include a high-speed random access memory, and may also include a non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some examples, the memory 104 may further include a memory remotely arranged relative to the processor 102, and these remote memories can be connected to the mobile terminal via a network. Examples of the above-mentioned network include but are not limited to the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof. The transmission device 106 is used to receive or send data via a network. The above-mentioned specific examples of the network may include a wireless network provided by a communication provider of the mobile terminal. In one example, the transmission device 106 includes a network adapter (Network Interface Controller, referred to as NIC), which can be connected to other network devices through a base station so as to communicate with the Internet. In one example, the transmission device 106 may be a radio frequency (RF) module, which is used to communicate with the Internet wirelessly.

[0030] In this embodiment, a path tracking control method for a tracked vehicle running on a mobile terminal, a computer terminal or a similar computing device is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0031] Figure 2 FIG. 1 is a flow chart of a path tracking control method for a tracked vehicle according to an embodiment of the present application. Figure 2 As shown, the method comprises the following steps:

[0032] Step S201, obtaining the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of obstacles, wherein the expected path is a preset driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle;

[0033] Specifically, the path planning layer in the unmanned driving system is responsible for generating an optimal path from the starting point to the end point based on the vehicle's starting point, end point and real-time environmental information. The desired path output by the path planning layer includes a series of target points, each of which contains coordinate information in the vehicle coordinate system. In certain specific environments, such as mines and agricultural scenes, the path may be pre-set and stored in the vehicle's control unit. These preset paths can be based on historical data, environmental mapping or manually set. The current position and status information of the tracked vehicle can be obtained through the Global Positioning System (GPS) and Inertial Measurement Unit (IMU), wheel speed sensor, visual sensor, etc. The obstacle position can be obtained through lidar, ultrasonic sensor, radar, visual sensor, etc.

[0034] Step S202, constructing a vehicle dynamics model based at least on the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving;

[0035] Specifically, the vehicle dynamics model is a mathematical model that describes the relationship between the vehicle's motion state (such as speed, acceleration, angular velocity, angular acceleration, etc.) and the forces acting on it (such as driving force, braking force, friction, air resistance, etc.). It is based on Newton's laws of motion or Lagrangian mechanics, and takes into account physical parameters such as the vehicle's mass, inertia, tire characteristics, suspension system, engine power output, and transmission system efficiency. The dynamics model not only describes the vehicle's translational motion, but also involves the vehicle's rotational motion, as well as the interaction between the vehicle and the ground. It is the basis for achieving precise control, predicting motion trajectories, and analyzing vehicle performance.

[0036] Step S203, constructing a safe driving corridor based at least on the expected path, the current position and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with the obstacle during path tracking;

[0037] Specifically, the safe driving corridor is a virtual boundary designed in autonomous driving technology to ensure the safety of vehicles in complex environments. It is a strategic planning space, defined as an area around the desired path, the size and shape of which are designed to avoid collisions between the vehicle and detected obstacles. The construction of the safe driving corridor takes into account the size, motion characteristics, location of obstacles and possible uncertainties of the vehicle to ensure the safety of the vehicle when driving in it.

[0038] Step S204, determining whether the current position is located in the safe driving corridor. If the current position is not located in the safe driving corridor, a dynamic window search control algorithm is used to determine an optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path.

[0039] Specifically, the Dynamic Window Approach (DWA) is a local path planning and obstacle avoidance algorithm widely used in the field of mobile robots, especially unmanned vehicles and automated guided vehicles (AGVs). Its core idea is to dynamically select an optimal speed control sequence in the context of the robot's current state (such as position, velocity and acceleration) and its physical and kinematic constraints to ensure that the robot can reach the target point safely and efficiently while avoiding collisions with static or dynamic obstacles. A key advantage of the DWA algorithm is that it can adjust the robot's behavior in real time in unknown or dynamically changing environments to cope with emergencies. It does not require the global information of the entire environment or the precise location of obstacles in advance, but makes decisions in local areas, which makes DWA particularly suitable for scenarios where complete environmental modeling is not possible or the environment is constantly changing. However, the computational efficiency and performance of the DWA algorithm largely depend on the definition of the dynamic window and the velocity sampling strategy, so in practical applications, it needs to be properly tuned according to the specific scenario.

[0040] Step S205, when the current position is within the safe driving corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to track the desired path according to the driving torque.

[0041] Specifically, the following path constraint algorithm is a control strategy that ensures that a mobile robot or autonomous vehicle can stay on a preset path during movement. This algorithm is usually used for path tracking tasks, aiming to make the actual trajectory of the vehicle as close as possible to a given desired path while considering the kinematic and dynamic constraints of the vehicle. The core of the following path constraint algorithm is to calculate the vehicle control quantity (such as speed, acceleration, steering angle, driving torque, etc.) to meet the position and direction requirements on the path while ensuring the stability and safety of the vehicle. The driving torque is the torque acting on the vehicle drive shaft to drive the vehicle forward or backward. In a tracked vehicle, the driving torque is generated by the drive motor and transmitted to the track through the transmission system, causing the track to rotate and propel the vehicle forward. The calculation of the driving torque usually involves the analysis of the vehicle dynamics model, which needs to consider many factors such as the vehicle's mass, inertia, the interaction between the tire or track and the ground, and the efficiency of the power system.

[0042] Through this embodiment, first, the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle are obtained, wherein the expected path is a preset driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; then, a vehicle dynamics model is constructed at least according to the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; then, a safe driving corridor is constructed at least according to the expected path, the current position and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with obstacles during path tracking; finally, it is determined whether the current position is located in the safe driving corridor. If the current position is not located in the safe driving corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path; if the current position is located in the safe driving corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to track the expected path according to the driving torque. The path tracking control method of the tracked vehicle at least includes three parts: vehicle dynamics model, safe driving corridor and path tracking control algorithm design. The vehicle dynamics model can fully combine the structural characteristics of the tracked vehicle, analyze its mechanical characteristics, and construct a kinematic and dynamic model. The safe driving corridor can take into account the safety of the tracked vehicle during the tracking process. The path tracking control algorithm designs a path tracking control algorithm based on dynamic window search when the initial condition vehicle is outside the safe corridor, and a path tracking control algorithm based on constraint following in the safe corridor. The tracked vehicle is controlled to run so that when the initial condition vehicle is outside the safe corridor, it can avoid obstacles and drive into the safe corridor, and accurately track the expected path in the safe corridor, thereby improving the safety and accuracy of the path tracking control and meeting the needs of actual operation scenarios. The problem of low safety and accuracy of the path tracking control of tracked vehicles in the prior art is solved.

[0043] In the specific implementation process, the above step S201 can be implemented by the following steps: step S2011, obtaining multiple target points, and connecting the multiple target points in sequence to form the above expected path; step S2012, according to the formula Determine the current position P veh , where x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system, is the heading angle of the tracked vehicle; step S2013, obtaining the center of mass linear velocity of the tracked vehicle, the angular velocity of the tracked vehicle, the rotation angle of the left track of the tracked vehicle, the linear velocity of the left track of the tracked vehicle, the rotation angle of the right track of the tracked vehicle and the linear velocity of the right track of the tracked vehicle, and obtaining the state of the tracked vehicle; step S2014, according to the formula: Determine the position P of the above obstacleobstacle , where x obs and obs is the position coordinate of the above obstacle in the geodetic coordinate system, The method can further obtain the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle.

[0044] In order to further quickly construct the above vehicle dynamics model, the above step S202 of the present application can be implemented by the following steps: Step S2021, according to the formula The Lagrangian equation L of the tracked vehicle is constructed, wherein m is the mass of the tracked vehicle, J is the moment of inertia about the center of mass of the tracked vehicle, ξ is the first state vector of the tracked vehicle, M is the inertia matrix of the tracked vehicle; Step S2022, according to the formula Construct the dynamic equation u(t) of the tracked vehicle, where C is the Coriolis centrifugal force of the tracked vehicle; Step S2023, according to the formula Construct the kinematic model of the tracked vehicle, where L is the distance between the left track and the right track of the tracked vehicle, b is the average track width of the tracked vehicle, θ L is the rotation angle of the left track of the above tracked vehicle, θ R is the rotation angle of the right track of the above-mentioned tracked vehicle, ζ is the second state vector of the above-mentioned tracked vehicle, ζ=[θ R ,θ L ] T , Step S2024: Substitute the kinematic model into the dynamic equation to obtain the vehicle dynamics model in, Wherein, u is the driving torque of the above tracked vehicle.

[0045] Specifically, a vehicle dynamics model is constructed at least based on the current position of the vehicle. Although the position itself is not a direct element in constructing a dynamics model, the current position information is crucial in vehicle control and state updating. The following are the basic steps for constructing a simple vehicle dynamics model, taking a two-wheel drive vehicle as an example: Define the coordinate system: First, you need to define a coordinate system. In most cases, a geodetic coordinate system or a vehicle body coordinate system is selected. The geodetic coordinate system usually has the north direction as the positive x-axis and the east direction as the positive y-axis, while the x-axis of the vehicle body coordinate system points to the vehicle's forward direction and the y-axis points to the vehicle's width direction. Determine state variables: State variables are a set of parameters that describe the vehicle's motion state, and should at least include the position (x, y).

[0046] The above step S203 can be implemented in other ways, for example: step S2031, according to the formula Calculating safe area Among them, D(P i des ,P veh ) is the current position P veh and the i-th target point P i des The Euclidean distance between the target points is the Euclidean distance between the target points, the circle with each target point as the center and R as the radius does not contain the obstacle, and the expected path includes the target points connected in sequence; step S2032, according to the formula Construct the above-mentioned safe driving corridor SafeZone, where n is the number of the above-mentioned target points.

[0047] In practical applications, such as Figure 3 As shown in the figure, it can be seen that when the target points on the expected path Path are dense enough, the circular safety area of ​​n target points can form a safe driving corridor, and the path tracking control strategy controls the vehicle to track the expected path in the safety corridor, thereby ensuring tracking safety. When the number of target points is not dense enough, the number of expected target points can be expanded by interpolation and path smoothing.

[0048] In some embodiments, after step S203 and before step S204, the above step can be implemented by the following steps: Step S206, according to formula e safezone =ln[R 2 -D 2 (P i des ,P veh )], calculate the safety constraint e of the above tracked vehicle safezone , where D(P i des ,P veh ) is the current position P veh and the i-th target point P i des The Euclidean distance between the above target points, the circle with each of the above target points as the center and R as the radius does not contain the above obstacles, and the above expected path includes multiple target points connected in sequence, x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system, is the heading angle of the above tracked vehicle, and is the position coordinate of the i-th target point in the geodetic coordinate system, is the heading angle of the i-th target point; Step S207, according to the formula Calculate the target constraint e of the above tracked vehicle target Step S208, when the safety constraint and the target constraint are both close to 0, the first state vector ξ of the tracked vehicle satisfies in,

[0049] This method can further improve the security of path tracking.

[0050] Specifically, when the above first-order and second-order equality constraints are satisfied, the safe area tracking error e safezone And the target point tracking error e target Both tend to 0, thus satisfying the safety constraints and target constraints, ensuring that the tracked vehicle tracks the target point P i des The tracked vehicle is always in the safe area during the process and accurately tracks the target point. The constraint is also met when tracking the next target point, so that the tracked vehicle always tracks the expected path within the defined safe driving corridor, thereby ensuring the safety of path tracking.

[0051] The above step S204 can be implemented by the following steps: Step S2041, determining step, according to V sample ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]}, determine the sampling range V of the above tracked vehicle sample , where v is the linear velocity of the center of mass of the tracked vehicle, ω is the angular velocity of the tracked vehicle, and v min is the minimum value of the center of mass linear velocity of the above tracked vehicle, v max is the maximum value of the center of mass linear velocity of the above tracked vehicle, ω min is the minimum angular velocity of the tracked vehicle, ω max is the maximum value of the angular velocity of the above tracked vehicle; Step S2042, calculation step, according to the formula: Calculate the number of samples N samp , wherein Δv is the sampling speed interval of the centroid linear velocity of the above-mentioned tracked vehicle, and Δω is the sampling speed interval of the angular velocity of the above-mentioned tracked vehicle; Step S2043, updating step, according to the formula For the above current position P veh Update to get the updated position P pre ,in, Δt is the sampling time interval of the centroid linear velocity of the tracked vehicle and the angular velocity of the tracked vehicle; Step S2044, the first repetition step, repeats the update step multiple times, and in the repetition process updates the current position to the updated position obtained in the last repetition process, until the preset number of times is reached, and N is obtained. samp Step S2045, calculation step, according to the formula E = g × D (P j ,P obstacle )+h×(π-Δθ), calculate the evaluation function E of each of the above preset trajectories, where g and h are weight coefficients, both g and h are greater than 0, D(P j ,P obstacle ) is the last position P in the above preset trajectory j With the above obstacle P obstacle The Euclidean distance between the positions, Δθ is P j The tangent direction of i des The angle of the connecting line, the above-mentioned expected path includes a plurality of the above-mentioned target points connected in sequence; step S2046, a determination step, determining that the above-mentioned preset trajectory corresponding to the maximum value of the plurality of the above-mentioned evaluation functions is the optimal trajectory, and obtaining the linear velocity of the above-mentioned tracked vehicle on the above-mentioned optimal trajectory and the angular velocity of the above-mentioned tracked vehicle on the above-mentioned optimal trajectory; step S2047, a first control step, according to the formula Calculate the left track speed v of the above tracked vehicle L , and control the left track of the above tracked vehicle to travel at the above left track speed, wherein v opt is the linear speed of the tracked vehicle on the optimal trajectory, L is the distance between the left track and the right track of the tracked vehicle, b is the average track width of the tracked vehicle, ω opt is the angular velocity of the tracked vehicle on the optimal trajectory; Step S2048, the second control step, according to the formula Calculate the right track speed v of the above tracked vehicle R , and control the right crawler of the above crawler vehicle to travel at the above right crawler speed; step S2049, the second repetition step, sequentially repeating the above determination step, the above calculation step, the above update step, the above first repetition step, the above calculation step, the above determination step, the above first control step and the above second control step at least once, until the above crawler vehicle enters the above safe driving corridor. This method can further enable the crawler vehicle to quickly enter the safe situation corridor.

[0052] The above step S205 can be implemented by the following steps: Step S2051, according to the formula Calculate the above driving torque in, M is the inertia matrix of the tracked vehicle, ζ is the second state vector of the tracked vehicle, ζ=[θ R ,θ L ] T ,θ L is the rotation angle of the left track of the above tracked vehicle, θ R is the rotation angle of the right track of the above tracked vehicle, C is the Coriolis centrifugal force of the above-mentioned tracked vehicle, and κ is an adjustable coefficient greater than 0; step S2052, the above-mentioned driving torque is brought into the above-mentioned vehicle dynamics model to obtain the first target linear velocity and the second target linear velocity, and the left track of the above-mentioned tracked vehicle is controlled to travel at the above-mentioned first target linear velocity, and the right track of the above-mentioned tracked vehicle is controlled to travel at the above-mentioned second target linear velocity.

[0053] Specifically, the specific steps for calculating the driving torque are as follows: First, a mathematical model is constructed based on the dynamic characteristics of the vehicle, as mentioned above, using the Lagrange equations or Newton's laws of motion. The model should include vehicle state variables (such as position, velocity, acceleration) and various forces and torques acting on the vehicle (such as driving torque, friction, air resistance, etc.). The vehicle's current position, velocity, acceleration, steering angle and other state information are obtained in real time, which is usually achieved through on-board sensors and control systems. According to the current state of the vehicle and the desired path, the deviation between the vehicle and the path is calculated. This includes lateral and longitudinal deviations, as well as the deviation between the vehicle direction and the path direction. A controller is designed that can output the required control amount based on the path deviation and the vehicle state. In path tracking control, the goal of the controller is to adjust the driving torque so that the vehicle can travel along the desired path while satisfying dynamic constraints such as maximum acceleration, maximum speed, etc. Using the designed controller algorithm, the driving torque required for the left and right tracks is calculated based on the current state of the vehicle and the path deviation. This may involve feedback control, predictive control or model-based control techniques such as PID control, model predictive control (MPC), etc. During the vehicle's driving process, the above steps are repeated continuously, and the driving torque is dynamically adjusted according to the real-time path deviation and vehicle status to ensure that the vehicle can continue to move along the desired path. When calculating the driving torque, safety constraints must be considered, such as avoiding collisions with obstacles and staying in the safety corridor. This is usually achieved by adding additional safety constraints or obstacle avoidance algorithms to the controller.

[0054] In order to enable those skilled in the art to more clearly understand the technical solution of the present application, the implementation process of the path tracking control method for the tracked vehicle of the present application will be described in detail below in combination with specific embodiments.

[0055] This embodiment relates to a specific path tracking control method for a tracked vehicle, such as Figure 4As shown, the following steps are included:

[0056] Step S1: Obtain the desired path, tracked vehicle position and state, and obstacle position;

[0057] Step S2: constructing a vehicle dynamics model, a safe driving corridor and equation constraints;

[0058] Step S3: Determine whether the vehicle position is within the safety corridor under the initial condition, if not, execute step S4, if yes, execute step S5;

[0059] Step S4: executing a path planning method based on dynamic window search, outputting the lateral speeds of the left and right tracks, and continuously updating the vehicle position;

[0060] Step S5: Execute the path planning method based on constraint following, output the driving torque, and control the vehicle to accurately track the desired path in the safety corridor.

[0061] The embodiment of the present application also provides a path tracking control device for a tracked vehicle. It should be noted that the path tracking control device for a tracked vehicle in the embodiment of the present application can be used to execute the path tracking control method for a tracked vehicle provided in the embodiment of the present application. The device is used to implement the above-mentioned embodiments and preferred implementation modes, and the descriptions that have been made will not be repeated. As used below, the term "module" can be a combination of software and / or hardware that implements a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.

[0062] The following is an introduction to the path tracking control device for a tracked vehicle provided in an embodiment of the present application.

[0063] Figure 5 Schematic diagram of a path tracking control device for a tracked vehicle according to an embodiment of the present application. Figure 5 As shown, the device comprises:

[0064] The acquisition unit 10 is used to acquire the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle, wherein the expected path is a preset driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle;

[0065] Specifically, the path planning layer in the unmanned driving system is responsible for generating an optimal path from the starting point to the end point based on the vehicle's starting point, end point and real-time environmental information. The desired path output by the path planning layer includes a series of target points, each of which contains coordinate information in the vehicle coordinate system. In certain specific environments, such as mines and agricultural scenes, the path may be pre-set and stored in the vehicle's control unit. These preset paths can be based on historical data, environmental mapping or manually set. The current position and status information of the tracked vehicle can be obtained through the Global Positioning System (GPS) and Inertial Measurement Unit (IMU), wheel speed sensor, visual sensor, etc. The obstacle position can be obtained through lidar, ultrasonic sensor, radar, visual sensor, etc.

[0066] A first construction unit 20 is used to construct a vehicle dynamics model at least according to the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving;

[0067] Specifically, the vehicle dynamics model is a mathematical model that describes the relationship between the vehicle's motion state (such as speed, acceleration, angular velocity, angular acceleration, etc.) and the forces acting on it (such as driving force, braking force, friction, air resistance, etc.). It is based on Newton's laws of motion or Lagrangian mechanics, and takes into account physical parameters such as the vehicle's mass, inertia, tire characteristics, suspension system, engine power output, and transmission system efficiency. The dynamics model not only describes the vehicle's translational motion, but also involves the vehicle's rotational motion, as well as the interaction between the vehicle and the ground. It is the basis for achieving precise control, predicting motion trajectories, and analyzing vehicle performance.

[0068] A second construction unit 30 is used to construct a safe driving corridor based on at least the expected path, the current position and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with the obstacle during path tracking;

[0069] Specifically, the safe driving corridor is a virtual boundary designed in autonomous driving technology to ensure the safety of vehicles in complex environments. It is a strategic planning space, defined as an area around the desired path, the size and shape of which are designed to avoid collisions between the vehicle and detected obstacles. The construction of the safe driving corridor takes into account the size, motion characteristics, location of obstacles and possible uncertainties of the vehicle to ensure the safety of the vehicle when driving in it.

[0070] The first control unit 40 is used to determine whether the current position is located in the safe driving corridor. If the current position is not located in the safe driving corridor, a dynamic window search control algorithm is used to determine an optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path.

[0071] Specifically, the Dynamic Window Approach (DWA) is a local path planning and obstacle avoidance algorithm widely used in the field of mobile robots, especially unmanned vehicles and automated guided vehicles (AGVs). Its core idea is to dynamically select an optimal speed control sequence in the context of the robot's current state (such as position, velocity and acceleration) and its physical and kinematic constraints to ensure that the robot can reach the target point safely and efficiently while avoiding collisions with static or dynamic obstacles. A key advantage of the DWA algorithm is that it can adjust the robot's behavior in real time in unknown or dynamically changing environments to cope with emergencies. It does not require the global information of the entire environment or the precise location of obstacles in advance, but makes decisions in local areas, which makes DWA particularly suitable for scenarios where complete environmental modeling is not possible or the environment is constantly changing. However, the computational efficiency and performance of the DWA algorithm largely depend on the definition of the dynamic window and the velocity sampling strategy, so in practical applications, it needs to be properly tuned according to the specific scenario.

[0072] The second control unit 50 is used to calculate the driving torque by adopting a following path constraint algorithm when the current position is within the safe driving corridor, and control the tracked vehicle to track the desired path according to the driving torque.

[0073] Specifically, the following path constraint algorithm is a control strategy that ensures that a mobile robot or autonomous vehicle can stay on a preset path during movement. This algorithm is usually used for path tracking tasks, aiming to make the actual trajectory of the vehicle as close as possible to a given desired path while considering the kinematic and dynamic constraints of the vehicle. The core of the following path constraint algorithm is to calculate the vehicle control quantity (such as speed, acceleration, steering angle, driving torque, etc.) to meet the position and direction requirements on the path while ensuring the stability and safety of the vehicle. The driving torque is the torque acting on the vehicle drive shaft to drive the vehicle forward or backward. In a tracked vehicle, the driving torque is generated by the drive motor and transmitted to the track through the transmission system, causing the track to rotate and propel the vehicle forward. The calculation of the driving torque usually involves the analysis of the vehicle dynamics model, which needs to consider many factors such as the vehicle's mass, inertia, the interaction between the tire or track and the ground, and the efficiency of the power system.

[0074] Through this embodiment, the acquisition unit acquires the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle, wherein the expected path is a preset driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; the first construction unit constructs a vehicle dynamics model at least according to the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; the second construction unit constructs a safe driving corridor at least according to the expected path, the current position and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with obstacles during path tracking; the first control unit determines whether the current position is located in the safe driving corridor, and when the current position is not located in the safe driving corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path; the second control unit calculates the driving torque using a following path constraint algorithm when the current position is located in the safe driving corridor, and controls the tracked vehicle to track the expected path according to the driving torque. The path tracking control method of the tracked vehicle at least includes three parts: vehicle dynamics model, safe driving corridor and path tracking control algorithm design. The vehicle dynamics model can fully combine the structural characteristics of the tracked vehicle, analyze its mechanical characteristics, and construct a kinematic and dynamic model. The safe driving corridor can take into account the safety of the tracked vehicle during the tracking process. The path tracking control algorithm designs a path tracking control algorithm based on dynamic window search when the initial condition vehicle is outside the safe corridor, and a path tracking control algorithm based on constraint following in the safe corridor. The tracked vehicle is controlled to run so that when the initial condition vehicle is outside the safe corridor, it can avoid obstacles and drive into the safe corridor, and accurately track the expected path in the safe corridor, thereby improving the safety and accuracy of the path tracking control and meeting the needs of actual operation scenarios. The problem of low safety and accuracy of the path tracking control of tracked vehicles in the prior art is solved.

[0075] In the specific implementation process, the acquisition unit includes a first acquisition module, a first determination module, a second acquisition module and a second determination module, wherein the first acquisition module is used to acquire multiple target points, and the multiple target points are connected in sequence to form the expected path; the first determination module is used to obtain the desired path according to the formula Determine the current position P veh , where x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system, is the heading angle of the tracked vehicle; the second acquisition module is used to obtain the center of mass linear velocity of the tracked vehicle, the angular velocity of the tracked vehicle, the rotation angle of the left track of the tracked vehicle, the linear velocity of the left track of the tracked vehicle, the rotation angle of the right track of the tracked vehicle, and the linear velocity of the right track of the tracked vehicle to obtain the state of the tracked vehicle; the second determination module is used to obtain the state of the tracked vehicle according to the formula Determine the position P of the above obstacle obstacle , where x obs and obs is the position coordinate of the above obstacle in the geodetic coordinate system, The device can further obtain the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle.

[0076] In order to further quickly construct the above-mentioned vehicle dynamics model, the above-mentioned first construction unit of the present application includes a first construction module, a second construction module, a third construction module and a first input module, wherein the first construction module is used to The Lagrangian equation L of the tracked vehicle is constructed, wherein m is the mass of the tracked vehicle, J is the moment of inertia about the center of mass of the tracked vehicle, ξ is the first state vector of the tracked vehicle, M is the inertia matrix of the tracked vehicle; the second building block is used according to the formula Construct the dynamic equation u(t) of the tracked vehicle, where C is the Coriolis centrifugal force of the tracked vehicle; the third construction module is used to calculate the dynamic equation u(t) of the tracked vehicle according to the formula Construct the kinematic model of the tracked vehicle, where L is the distance between the left track and the right track of the tracked vehicle, b is the average track width of the tracked vehicle, θ L is the rotation angle of the left track of the above tracked vehicle, θ R is the rotation angle of the right track of the above-mentioned tracked vehicle, ζ is the second state vector of the above-mentioned tracked vehicle, ζ=[θ R ,θ L ] T , The first input module is used to substitute the above kinematic model into the above dynamic equation to obtain the above vehicle dynamic model in, Wherein, u is the driving torque of the above tracked vehicle.

[0077] Specifically, a vehicle dynamics model is constructed at least based on the current position of the vehicle. Although the position itself is not a direct element in constructing a dynamics model, the current position information is crucial in vehicle control and state updating. The following are the basic steps for constructing a simple vehicle dynamics model, taking a two-wheel drive vehicle as an example: Define the coordinate system: First, you need to define a coordinate system. In most cases, a geodetic coordinate system or a vehicle body coordinate system is selected. The geodetic coordinate system usually has the north direction as the positive x-axis and the east direction as the positive y-axis, while the x-axis of the vehicle body coordinate system points to the vehicle's forward direction and the y-axis points to the vehicle's width direction. Determine state variables: State variables are a set of parameters that describe the vehicle's motion state, and should at least include the position (x, y).

[0078] The second construction unit includes a first calculation module and a fourth construction module, wherein the first calculation module is used to calculate the value of Calculating safe area Among them, D(P i des ,P veh ) is the current position P veh and the i-th target point P i des The Euclidean distance between the target points is the Euclidean distance between the target points, the circle with each target point as the center and R as the radius does not contain the obstacle, and the expected path includes the multiple target points connected in sequence; the fourth construction module is used to calculate the target point according to the formula Construct the above-mentioned safe driving corridor SafeZone, where n is the number of the above-mentioned target points.

[0079] In practical applications, such as Figure 3 As shown in the figure, it can be seen that when the target points on the expected path Path are dense enough, the circular safety area of ​​n target points can form a safe driving corridor, and the path tracking control strategy controls the vehicle to track the expected path in the safety corridor, thereby ensuring tracking safety. When the number of target points is not dense enough, the number of expected target points can be expanded by interpolation and path smoothing.

[0080] In some embodiments, the above device further includes a first calculation unit, a second calculation unit and a processing unit, wherein the first calculation unit is used to calculate the value of the first calculation unit according to the formula e safezone =ln[R 2 -D 2 (P i des ,P veh )], calculate the safety constraint e of the above tracked vehicle safezone , where D(P i des ,P veh ) is the current position P veh and the i-th target point Pi des The Euclidean distance between the above target points, the circle with each of the above target points as the center and R as the radius does not contain the above obstacles, and the above expected path includes multiple target points connected in sequence, x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system, is the heading angle of the above tracked vehicle, and is the position coordinate of the i-th target point in the geodetic coordinate system, is the heading angle of the i-th target point; the second calculation unit is used according to the formula: Calculate the target constraint e of the above tracked vehicle target ; A processing unit for, when the safety constraint and the target constraint approach 0 at the same time, the first state vector ξ of the tracked vehicle satisfies in, This device can further improve the safety of path tracking.

[0081] Specifically, when the above first-order and second-order equality constraints are satisfied, the safe area tracking error e sa f ezone And the target point tracking error e target Both tend to 0, thus satisfying the safety constraints and target constraints, ensuring that the tracked vehicle tracks the target point P i des The tracked vehicle is always in the safe area during the process and accurately tracks the target point. The constraint is also met when tracking the next target point, so that the tracked vehicle always tracks the expected path within the defined safe driving corridor, thereby ensuring the safety of path tracking.

[0082] The first control unit comprises: a third determining module, a second calculating module, an updating module, a first repeating module, a third calculating module, a fourth determining module, a first control module, a second control module and a second repeating module, wherein the third determining module is used to determine the step, according to V sample ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]}, determine the sampling range V of the above tracked vehicle sample , where v is the linear velocity of the center of mass of the tracked vehicle, ω is the angular velocity of the tracked vehicle, and v min is the minimum value of the center of mass linear velocity of the above tracked vehicle, v max is the maximum value of the center of mass linear velocity of the above tracked vehicle, ω min is the minimum angular velocity of the tracked vehicle, ωmax is the maximum value of the angular velocity of the tracked vehicle; the second calculation module is used for the calculation step, according to the formula Calculate the number of samples N samp , where Δv is the sampling speed interval of the center of mass linear velocity of the above-mentioned tracked vehicle, and Δω is the sampling speed interval of the angular velocity of the above-mentioned tracked vehicle; the update module is used for the update step, according to the formula For the above current position P veh Update to get the updated position P pre ,in, Δt is the sampling time interval of the centroid linear velocity of the tracked vehicle and the angular velocity of the tracked vehicle; the first repetition module is used for the first repetition step, repeating the update step multiple times, and updating the current position to the updated position obtained in the last repetition process in the repetition process until the preset number of times is reached, and N is obtained. s a m The third calculation module is used to calculate the steps according to the formula E = g × D (P j ,P obstacle )+h×(π-Δθ), calculate the evaluation function E of each of the above preset trajectories, where g and h are weight coefficients, g and h are both greater than 0, D(P j ,P obstacle ) is the last position P in the above preset trajectory j With the above obstacle P obstacle The Euclidean distance between the positions, Δθ is P j The tangent direction of i des The angle of the connecting line, the above-mentioned expected path includes a plurality of the above-mentioned target points connected in sequence; the fourth determination module is used for the determination step, determining the above-mentioned preset trajectory corresponding to the maximum value of the plurality of the above-mentioned evaluation functions as the optimal trajectory, and obtaining the linear speed of the above-mentioned tracked vehicle on the above-mentioned optimal trajectory and the angular velocity of the above-mentioned tracked vehicle on the above-mentioned optimal trajectory; the first control module is used for the first control step, according to the formula Calculate the left track speed v of the above tracked vehicle L , and control the left track of the above tracked vehicle to travel at the above left track speed, wherein v opt is the linear speed of the tracked vehicle on the optimal trajectory, L is the distance between the left track and the right track of the tracked vehicle, b is the average track width of the tracked vehicle, ω opt is the angular velocity of the tracked vehicle in the optimal trajectory; the second control module is used in the second control step, according to the formula Calculate the right track speed v of the above tracked vehicle R, and controls the right crawler of the above crawler vehicle to travel at the above right crawler speed; the second repeating module is used for the second repeating step, and sequentially repeats the above determining step, the above calculating step, the above updating step, the above first repeating step, the above calculating step, the above determining step, the above first controlling step and the above second controlling step at least once, until the above crawler vehicle enters the above safe driving corridor. The device can further enable the crawler vehicle to quickly enter the safe situation corridor.

[0083] The second control unit includes a fourth calculation module and a second input module, wherein the fourth calculation module is used to calculate the value of Calculate the above driving torque in, M is the inertia matrix of the tracked vehicle, ζ is the second state vector of the tracked vehicle, ζ=[θ R ,θ L ] T ,θ L is the rotation angle of the left track of the above tracked vehicle, θ R is the rotation angle of the right track of the above tracked vehicle, C is the Coriolis centrifugal force of the above-mentioned tracked vehicle, and κ is an adjustable coefficient greater than 0; the second input module is used to bring the above-mentioned driving torque into the above-mentioned vehicle dynamics model to obtain the first target linear velocity and the second target linear velocity, and control the left track of the above-mentioned tracked vehicle to travel at the above-mentioned first target linear velocity, and control the right track of the above-mentioned tracked vehicle to travel at the above-mentioned second target linear velocity.

[0084] Specifically, the specific steps for calculating the driving torque are as follows: First, a mathematical model is constructed based on the dynamic characteristics of the vehicle, as mentioned above, using the Lagrange equations or Newton's laws of motion. The model should include vehicle state variables (such as position, velocity, acceleration) and various forces and torques acting on the vehicle (such as driving torque, friction, air resistance, etc.). The vehicle's current position, velocity, acceleration, steering angle and other state information are obtained in real time, which is usually achieved through on-board sensors and control systems. According to the current state of the vehicle and the desired path, the deviation between the vehicle and the path is calculated. This includes lateral and longitudinal deviations, as well as the deviation between the vehicle direction and the path direction. A controller is designed that can output the required control amount based on the path deviation and the vehicle state. In path tracking control, the goal of the controller is to adjust the driving torque so that the vehicle can travel along the desired path while satisfying dynamic constraints such as maximum acceleration, maximum speed, etc. Using the designed controller algorithm, the driving torque required for the left and right tracks is calculated based on the current state of the vehicle and the path deviation. This may involve feedback control, predictive control or model-based control techniques such as PID control, model predictive control (MPC), etc. During the vehicle's driving process, the above steps are repeated continuously, and the driving torque is dynamically adjusted according to the real-time path deviation and vehicle status to ensure that the vehicle can continue to move along the desired path. When calculating the driving torque, safety constraints must be considered, such as avoiding collisions with obstacles and staying in the safety corridor. This is usually achieved by adding additional safety constraints or obstacle avoidance algorithms to the controller.

[0085] The path tracking control device for the tracked vehicle includes a processor and a memory. The acquisition unit, the first construction unit, the second construction unit, the first control unit, and the second control unit are all stored in the memory as program units, and the processor executes the program units stored in the memory to implement corresponding functions. The modules are all located in the same processor; or, the modules are located in different processors in any combination.

[0086] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0087] An embodiment of the present invention provides a computer-readable storage medium, which includes a stored program, wherein when the program is running, the device where the computer-readable storage medium is located is controlled to execute the path tracking control method of the tracked vehicle.

[0088] Obviously, those skilled in the art should understand that the above modules or steps of the present invention can be implemented by a general computing device, they can be concentrated on a single computing device, or distributed on a network composed of multiple computing devices, they can be implemented by a program code executable by a computing device, so that they can be stored in a storage device and executed by the computing device, and in some cases, the steps shown or described can be executed in a different order than here, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. Thus, the present invention is not limited to any specific combination of hardware and software.

[0089] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0090] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0091] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0092] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0093] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0094] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0095] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0096] From the above description, it can be seen that the above embodiments of the present application achieve the following technical effects:

[0097] 1) The path tracking control method of the tracked vehicle of the present application first obtains the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle, wherein the expected path is a pre-set driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; then, at least according to the current position, a vehicle dynamics model is constructed, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; then, at least according to the expected path, the current position and the position of the obstacle, a safe driving corridor is constructed, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with obstacles during path tracking; finally, it is determined whether the current position is located in the safe driving corridor. If the current position is not located in the safe driving corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path; if the current position is located in the safe driving corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to track the expected path according to the driving torque. The path tracking control method of the tracked vehicle at least includes three parts: vehicle dynamics model, safe driving corridor and path tracking control algorithm design. The vehicle dynamics model can fully combine the structural characteristics of the tracked vehicle, analyze its mechanical characteristics, and construct a kinematic and dynamic model. The safe driving corridor can take into account the safety of the tracked vehicle during the tracking process. The path tracking control algorithm designs a path tracking control algorithm based on dynamic window search when the initial condition vehicle is outside the safe corridor, and a path tracking control algorithm based on constraint following in the safe corridor. The tracked vehicle is controlled to run so that when the initial condition vehicle is outside the safe corridor, it can avoid obstacles and drive into the safe corridor, and accurately track the expected path in the safe corridor, thereby improving the safety and accuracy of the path tracking control and meeting the needs of actual operation scenarios. The problem of low safety and accuracy of the path tracking control of tracked vehicles in the prior art is solved.

[0098] 2) The path tracking control device of the tracked vehicle of the present application, the acquisition unit acquires the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle and the position of the obstacle, wherein the expected path is a pre-set driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; the first construction unit constructs a vehicle dynamics model based at least on the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; the second construction unit further constructs a safe driving corridor based at least on the expected path, the current position and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with obstacles during path tracking; the first control unit determines whether the current position is located in the safe driving corridor, and if the current position is not located in the safe driving corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path; the second control unit calculates the driving torque based on the following path constraint algorithm when the current position is located in the safe driving corridor, and controls the tracked vehicle to track the expected path according to the driving torque. The path tracking control method of the tracked vehicle at least includes three parts: vehicle dynamics model, safe driving corridor and path tracking control algorithm design. The vehicle dynamics model can fully combine the structural characteristics of the tracked vehicle, analyze its mechanical characteristics, and construct a kinematic and dynamic model. The safe driving corridor can take into account the safety of the tracked vehicle during the tracking process. The path tracking control algorithm designs a path tracking control algorithm based on dynamic window search when the initial condition vehicle is outside the safe corridor, and a path tracking control algorithm based on constraint following in the safe corridor. The tracked vehicle is controlled to run so that when the initial condition vehicle is outside the safe corridor, it can avoid obstacles and drive into the safe corridor, and accurately track the expected path in the safe corridor, thereby improving the safety and accuracy of the path tracking control and meeting the needs of actual operation scenarios. The problem of low safety and accuracy of the path tracking control of tracked vehicles in the prior art is solved.

[0099] The above description is only the preferred embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A path tracking control method for a tracked vehicle, characterized in that: include: Acquire the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of obstacles, wherein the expected path is a preset driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; At least according to the current position, construct a vehicle dynamics model, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; constructing a safe driving corridor based at least on the expected path, the current position and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with the obstacle during path tracking; Determining whether the current position is located in the safe driving corridor, and if the current position is not located in the safe driving corridor, using a dynamic window search control algorithm to determine an optimal path, and controlling the tracked vehicle to enter the safe driving corridor according to the optimal path; When the current position is within the safe driving corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to track the desired path according to the driving torque.

2. The method according to claim 1, characterized in that Obtaining the expected path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of obstacles, including: Acquire multiple target points, and connect the multiple target points in sequence to form the expected path; According to the formula Determine the current position P veh , where x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system, is the heading angle of the tracked vehicle; Obtaining the center-of-mass linear velocity of the tracked vehicle, the angular velocity of the tracked vehicle, the rotation angle of the left track of the tracked vehicle, the linear velocity of the left track of the tracked vehicle, the rotation angle of the right track of the tracked vehicle, and the linear velocity of the right track of the tracked vehicle to obtain the state of the tracked vehicle; According to the formula Determine the position P of the obstacle obstacle , where x obs and obs is the position coordinate of the obstacle in the geodetic coordinate system, is the heading angle of the obstacle.

3. The method according to claim 2, characterized in that Based at least on the current position, construct a vehicle dynamics model, including: According to the formula The Lagrangian equation L of the tracked vehicle is constructed, wherein m is the mass of the tracked vehicle, J is the moment of inertia about the center of mass of the tracked vehicle, ξ is the first state vector of the tracked vehicle, M is the inertia matrix of the tracked vehicle; According to the formula: Constructing the dynamic equation u(t) of the tracked vehicle, wherein C is the Coriolis centrifugal force of the tracked vehicle; According to the formula Construct a kinematic model of the tracked vehicle, where L is the distance between the left track and the right track of the tracked vehicle, b is the average track width of the tracked vehicle, θ L is the rotation angle of the left track of the tracked vehicle, θ R is the rotation angle of the right track of the tracked vehicle, ζ is the second state vector of the tracked vehicle, ζ=[θ R ,θ L ] T , Substituting the kinematic model into the dynamic equation, the vehicle dynamics model is obtained. in, Wherein, u is the driving torque of the tracked vehicle.

4. The method according to claim 1, characterized in that: Constructing a safe driving corridor based at least on the desired path, the current position, and the position of the obstacle, including: According to the formula Calculating safe area Among them, D(P i des ,P veh ) is the current position P veh and the i-th target point P i des The Euclidean distance between the target points, the circle with each target point as the center and R as the radius does not contain the obstacle, and the expected path includes multiple target points connected in sequence; According to the formula The safe driving corridor SafeZone is constructed, wherein n is the number of the target points.

5. The method according to claim 1, characterized in that After constructing a safe driving corridor based at least on the expected path, the current position, and the position of the obstacle, and before determining whether the current position is located within the safe driving corridor, the method further includes: According to formula e safezone =ln[R 2 -D 2 (P i des ,P veh )], calculate the safety constraint e of the tracked vehicle safezone , where D(P i des ,P veh ) is the current position P veh and the i-th target point P i des The Euclidean distance between the target points, the circle with each target point as the center and R as the radius does not contain the obstacle, and the expected path includes multiple target points connected in sequence, x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system, is the heading angle of the tracked vehicle, and is the position coordinate of the i-th target point in the geodetic coordinate system, is the heading angle of the i-th target point; According to the formula: Calculate the target constraint e of the tracked vehicle target ; When the safety constraint and the target constraint are both close to 0, the first state vector of the tracked vehicle in, 6. The method according to claim 1, characterized in that In the case where the current position is not within the safe driving corridor, a dynamic window search control algorithm is used to determine an optimal path, and the tracked vehicle is controlled to enter the safe driving corridor according to the optimal path, including: Determine the steps according to V sample ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]}, determine the sampling range V of the tracked vehicle sample , where v is the linear velocity of the center of mass of the tracked vehicle, ω is the angular velocity of the tracked vehicle, and v min is the minimum value of the center of mass linear velocity of the tracked vehicle, v max is the maximum value of the center-of-mass linear velocity of the tracked vehicle, ω min is the minimum angular velocity of the tracked vehicle, ω max is the maximum value of the angular velocity of the tracked vehicle; Calculation steps, according to the formula: Calculate the number of samples N samp , wherein Δv is the sampling speed interval of the center-of-mass linear velocity of the tracked vehicle, and Δω is the sampling speed interval of the angular velocity of the tracked vehicle; Update step, according to the formula For the current position P veh Update to get the updated position P pre ,in, Δt is the sampling time interval of the linear velocity of the center of mass of the tracked vehicle and the angular velocity of the tracked vehicle; The first repetition step repeats the updating step multiple times, and in the repetition process updates the current position to the updated position obtained in the last repetition process, until the preset number of times is reached, and N samp Preset tracks; Calculation steps, according to the formula E = g × D (P j ,P obstacle )+h×(π-Δθ), calculate the evaluation function E of each preset trajectory, where g and h are weight coefficients, g and h are both greater than 0, D(P j ,P obstacle ) is the last position P in the preset trajectory j With the obstacle P obstacle The Euclidean distance between the positions, Δθ is P j The tangent direction of i des The expected path includes a plurality of target points connected in sequence; A determination step, determining that the preset trajectory corresponding to the maximum value among the plurality of evaluation functions is the optimal trajectory, and obtaining the linear velocity of the tracked vehicle on the optimal trajectory and the angular velocity of the tracked vehicle on the optimal trajectory; The first control step is based on the formula Calculate the left track speed v of the tracked vehicle L , and controlling the left track of the tracked vehicle to travel at the left track speed, wherein v opt is the linear speed of the tracked vehicle on the optimal trajectory, L is the distance between the left track and the right track of the tracked vehicle, b is the average track width of the tracked vehicle, ω opt is the angular velocity of the tracked vehicle on the optimal trajectory; The second control step is based on the formula Calculate the right track speed v of the tracked vehicle R , and controlling the right crawler track of the crawler vehicle to travel at the right crawler track speed; The second repetition step is to repeat the determination step, the calculation step, the update step, the first repetition step, the calculation step, the determination step, the first control step and the second control step at least once in sequence until the tracked vehicle enters the safe driving corridor.

7. The method according to claim 5, characterized in that When the current position is within the safe driving corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to track the desired path according to the driving torque, including: According to the formula: Calculate the driving torque in, M is the inertia matrix of the tracked vehicle, ζ is the second state vector of the tracked vehicle, ζ=[θ R ,θ L ] T ,θ L is the rotation angle of the left track of the tracked vehicle, θ R is the rotation angle of the right track of the tracked vehicle, C is the Coriolis centrifugal force of the tracked vehicle, and κ is an adjustable coefficient greater than 0; The driving torque is introduced into the vehicle dynamics model to obtain a first target linear speed and a second target linear speed, and the left track of the tracked vehicle is controlled to travel at the first target linear speed, and the right track of the tracked vehicle is controlled to travel at the second target linear speed.

8. A path tracking control device for a tracked vehicle, characterized in that: include: an acquisition unit, used to acquire an expected path of the tracked vehicle, a current position of the tracked vehicle, a state of the tracked vehicle, and a position of an obstacle, wherein the expected path is a preset driving route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; A first construction unit is used to construct a vehicle dynamics model at least according to the current position, wherein the vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during driving; a second construction unit, configured to construct a safe driving corridor based at least on the expected path, the current position, and the position of the obstacle, wherein the safe driving corridor is used to control the tracked vehicle to avoid collision with the obstacle during path tracking; A first control unit, configured to determine whether the current position is located in the safe driving corridor, and if the current position is not located in the safe driving corridor, determine an optimal path using a dynamic window search control algorithm, and control the tracked vehicle to enter the safe driving corridor according to the optimal path; The second control unit is used to calculate the driving torque by adopting a following path constraint algorithm when the current position is within the safe driving corridor, and control the tracked vehicle to track the desired path according to the driving torque.

9. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored program, wherein when the program is executed, the device where the computer-readable storage medium is located is controlled to execute the method according to any one of claims 1 to 7.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images