Track following control method, control device, and storage medium for tracked vehicle

Abstract

The application provides a path tracking control method and device of a tracked vehicle and a storage medium. The method comprises the following steps: 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; constructing a vehicle dynamics model according to at least the current position; constructing a safe driving corridor according to at least the desired path, the current position and the position of the obstacle; in the case that the current position is not located in the safe driving corridor, determining an optimal path by using a dynamic window search control algorithm, and controlling the tracked vehicle to enter the safe driving corridor according to the optimal path; in the case that the current position is located in the safe driving corridor, calculating a driving torque by using a follow-path constraint algorithm, and controlling the tracked vehicle to track the desired path according to the driving torque. The problem that the safety and accuracy of path tracking control of the tracked vehicle in the prior art are not high is solved.

Description

Technical Field

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

[0002] Path tracking control is a key technology for unmanned tracked vehicles. It controls the rotational speed of the left and right track motors to follow a desired obstacle avoidance path, achieving autonomous tracking. Existing path tracking control methods primarily focus on tracking accuracy, but in complex driving environments, overshooting can easily occur, making it difficult to effectively control the error between the actual and desired paths, thus affecting the safety and stability of unmanned tracked vehicle path tracking. Therefore, designing a path tracking algorithm that considers the kinematics and dynamics of tracked vehicles, has low complexity, and balances tracking safety and accuracy is of great significance for the practical application of unmanned tracked vehicles. Summary of the Invention

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

[0004] According to one aspect of this application, a path tracking control method for a tracked vehicle is provided, comprising: acquiring a desired path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of an obstacle, wherein the desired path is a pre-set travel route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; 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 travel; constructing a safe travel corridor based at least on the desired path, the current position, and the position of the obstacle, wherein the safe travel corridor is used to control the tracked vehicle to avoid collisions with the obstacle during path tracking; determining whether the current position is within the safe travel corridor; if the current position is not within the safe travel corridor, using a dynamic window search control algorithm to determine an optimal path, and controlling the tracked vehicle to enter the safe travel corridor according to the optimal path; if the current position is within the safe travel corridor, using a following path constraint algorithm to calculate a driving torque, and controlling the tracked vehicle to follow the desired path according to the driving torque.

[0005] Optionally, obtaining the desired path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of obstacles includes: obtaining multiple target points, connecting the multiple target points sequentially to form 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. The heading angle of the tracked vehicle is given; the linear velocity of the center of mass 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 are obtained to determine the state of the tracked vehicle; according to the formula: Determine the position P of the obstacle. obstacle , where x obs and y obs The coordinates of the obstacle in the geodetic coordinate system are: The heading angle of the obstacle.

[0006] Optionally, at least based on the current position, a vehicle dynamics model is constructed, including: according to the formula Construct the Lagrange equation L for the tracked vehicle, where m is the mass of the tracked vehicle, J is the moment of inertia about the center of mass of the tracked vehicle, and ξ is the first state vector of the tracked vehicle. M is the inertia matrix of the tracked vehicle; according to the formula The dynamic equation u(t) of the tracked vehicle is constructed, 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 and right tracks of the tracked vehicle, b is the average track width of the tracked vehicle, and θ L Let θ be the rotation angle of the left track of the tracked vehicle. R Let ζ be the rotation angle of the right track of the tracked vehicle, and ζ be the second state vector of the tracked vehicle, ζ = [θ]. R ,θ L ] T , Substituting the kinematic model into the dynamic equation, we obtain the vehicle dynamic model. in, Where u is the driving torque of the tracked vehicle.

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

[0008] Optionally, after constructing a safe driving corridor based at least on the desired path, the current location, and the location of the obstacle, and before determining whether the vehicle's location is 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 ) represents the current position P veh With the i-th target point P i des The Euclidean distance between them, with each target point as the center and R as the radius, does not include the obstacles in the circle. The desired path includes multiple target points connected sequentially. x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system. Let be the heading angle of the tracked vehicle. and Let be the position coordinates of the i-th target point in the geodetic coordinate system. Let be the heading angle of the i-th target point; according to the formula Calculate the target constraint e of the tracked vehicle target When both the safety constraint and the target constraint approach 0, the first state vector ξ of the tracked vehicle satisfies... in,

[0009] Optionally, if the current position is not within the safe travel corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe travel corridor according to the optimal path, including: a determination step, based on 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 v is the minimum linear velocity of the center of mass of the tracked vehicle. max ω is the maximum linear velocity of the center of mass of the tracked vehicle. min ω is the minimum angular velocity of the tracked vehicle. m a x The maximum angular velocity of the tracked vehicle; calculation steps, according to the formula: Calculate the number of samples N samp Where Δv is the sampling velocity interval of the linear velocity of the center of mass of the tracked vehicle, and Δω is the sampling velocity interval of the angular velocity of the tracked vehicle; the update step is based on the formula: For the current position P veh Update to obtain the update position P pre ,in, Δt is the sampling time interval between the linear velocity of the center of mass of the tracked vehicle and the angular velocity of the tracked vehicle; the first repetition step involves repeating the update step multiple times, updating the current position to the updated position obtained in the previous repetition during the repetition process, until a preset number of times is reached to obtain N. samp A preset trajectory; calculation steps, according to the formula E=g×D(P) j ,P obstacle )+h×(π-Δθ), calculate the evaluation function E for each of the preset trajectories, where g and h are weight coefficients, both g and h are greater than 0, D(P j ,P obstacle ) represents 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 the i-th target point P i desThe angle between the lines connecting the desired path and the target points connected in sequence; the determination step, determining the preset trajectory corresponding to the maximum value among the multiple evaluation functions as the optimal trajectory, and obtaining the linear velocity and angular velocity of the tracked vehicle on the optimal trajectory; the first control step, according to the formula Calculate the speed v of the left track of the tracked vehicle. L And control the left track of the tracked vehicle to travel at the speed of the left track, where v opt Let L be the linear velocity of the tracked vehicle on the optimal trajectory, L be the distance between the left and right tracks of the tracked vehicle, b be the average track width of the tracked vehicle, and ω be the linear velocity of the tracked vehicle on the optimal trajectory. opt The angular velocity of the tracked vehicle on the optimal trajectory; the second control step, according to the formula Calculate the speed v of the right track of the tracked vehicle. R The system controls the right track of the tracked vehicle to travel at the right track speed; the second repeating step involves sequentially repeating the determining step, the calculating step, the updating step, the first repeating 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, 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 follow the desired path based on 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, and ζ = [θ R ,θ L ] T θ L Let θ be the rotation angle of the left track of the tracked vehicle. R Let be 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 this application, a path tracking control device for a tracked vehicle is provided, comprising: an acquisition unit, configured to acquire a desired path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of an obstacle, wherein the desired path is a pre-set travel route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; a first construction unit, configured to construct 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 travel; and a second construction unit, configured to construct a path tracking control device 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 collisions with obstacles during path tracking; a first control unit is used to determine whether the current position is within the safe driving corridor, and 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; a second control unit is used to calculate the driving torque using a following path constraint algorithm if the current position is within the safe driving corridor, and the tracked vehicle is controlled to follow the desired path according to the driving torque.

[0012] According to another aspect of this application, a computer-readable storage medium is provided, the computer-readable storage medium including a stored program, wherein, when the program is executed, it controls the device on which the computer-readable storage medium is located to perform any of the methods described.

[0013] According to another aspect of this application, a computer program product is provided, comprising a computer program that, when executed by a processor, implements the steps of any of the methods described.

[0014] Applying the technical solution of this application, firstly, the desired 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. The desired path is a pre-set travel route for the tracked vehicle, and the state characterizes the motion characteristics of the tracked vehicle. Then, at least based on the current position, a vehicle dynamics model is constructed, which characterizes the mechanical behavior and performance of the tracked vehicle during travel. Next, at least based on the desired path, the current position, and the position of the obstacle, a safe travel corridor is constructed. This safe travel corridor is used to control the tracked vehicle to avoid collisions with obstacles during path tracking. Finally, it is determined whether the current position is within the safe travel corridor. If the current position is not within the safe travel corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe travel corridor based on the optimal path. If the current position is within the safe travel corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to follow the desired path based on the driving torque. The path tracking control method for this tracked vehicle comprises at least three parts: a vehicle dynamics model, a safe driving corridor, and a path tracking control algorithm. The vehicle dynamics model fully incorporates the structural characteristics of the tracked vehicle, analyzes its mechanical properties, and constructs a kinematic and dynamic model. The safe driving corridor considers the safety of the tracked vehicle during the tracking process. The path tracking control algorithm includes two scenarios: a dynamic window search-based algorithm for the initial condition where the vehicle is outside the safe corridor, and a constraint-following-based algorithm for the vehicle within the safe corridor. These algorithms control the tracked vehicle's movement, enabling it to avoid obstacles and enter the safe corridor when initially outside, and accurately track the desired path within the safe corridor. This improves the safety and accuracy of path tracking control, meeting the requirements of actual operational scenarios. It solves the problem of insufficient safety and accuracy in the path tracking control of tracked vehicles in existing technologies. Attached Figure Description

[0015] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0016] Figure 1 A hardware structure block diagram of a mobile terminal for performing a path tracking control method for a tracked vehicle, according to an embodiment of this application, is shown.

[0017] Figure 2 A schematic flowchart of a path tracking control method for a tracked vehicle according to an embodiment of this application is shown.

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

[0019] Figure 4 A detailed flowchart of a path tracking control method for a tracked vehicle according to an embodiment of this 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 this application is shown.

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

[0022] 102. Processor; 104. Memory; 106. Transmission device; 108. Input / output device. Detailed Implementation

[0023] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0024] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort 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, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of this application described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

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

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

[0028] The methods and embodiments provided in this application can be executed on a mobile terminal, computer terminal, or similar computing device. Taking running on a mobile terminal as an example, Figure 1 This is a hardware structure block diagram of a mobile terminal for a path tracking control method for a tracked vehicle according to an embodiment of the present invention. Figure 1 As shown, a mobile terminal may include one or more ( Figure 1 Only one is shown in the diagram. A processor 102 (which may include, but is not limited to, a microprocessor MCU or a programmable logic device FPGA, etc.) and a memory 104 for storing data are also shown. The mobile terminal may further include a transmission device 106 for communication functions and an input / output device 108. Those skilled in the art will understand that... Figure 1 The structure shown is for illustrative purposes only and does not limit the structure of the mobile terminal described above. For example, the mobile terminal may also include components that are more... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown.

[0029] The memory 104 can be used to store computer programs, such as application software programs and modules, like the computer program corresponding to the path tracking control method for tracked vehicles in this embodiment of the invention. The processor 102 executes various functional applications and data processing by running the computer program stored in the memory 104, thereby implementing the above-described method. The memory 104 may include high-speed random access memory and non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some instances, the memory 104 may further include memory remotely located relative to the processor 102, and these remote memories can be connected to the mobile terminal via a network. Examples of the aforementioned networks include, but are not limited to, the Internet, corporate intranets, local area networks, mobile communication networks, and combinations thereof. The transmission device 106 is used to receive or send data via a network. Specific examples of the aforementioned networks may include wireless networks provided by the mobile terminal's communication provider. In one example, the transmission device 106 includes a network interface controller (NIC), which can be connected to other network devices via a base station 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] This embodiment provides a path tracking control method for a tracked vehicle that runs on a mobile terminal, computer terminal, or similar computing device. It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. Also, although a logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than that shown here.

[0031] Figure 2 This is a flowchart of a path tracking control method for a tracked vehicle according to an embodiment of this application. Figure 2 As shown, the method includes the following steps:

[0032] Step S201: Obtain the desired path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of the obstacle. The desired path is a pre-set travel 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 an autonomous driving system is responsible for generating an optimal path from the vehicle's starting point to its destination, based on the vehicle's origin, destination, and real-time environmental information. The desired path output by the path planning layer includes a series of target points, each containing coordinate information in the vehicle's coordinate system. In certain specific environments, such as mining or agricultural scenarios, paths may be pre-set and stored in the vehicle's control unit. These pre-set paths can be based on historical data, environmental mapping, or manually defined. The tracked vehicle's current position and status information can be obtained through a Global Positioning System (GPS), Inertial Measurement Unit (IMU), wheel speed sensors, and vision sensors. Obstacle locations can be obtained through lidar, ultrasonic sensors, radar, and vision sensors.

[0034] Step S202: Based at least on 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 travel.

[0035] Specifically, a vehicle dynamics model is a mathematical model that describes the relationship between a vehicle's motion (such as velocity, acceleration, angular velocity, angular acceleration, etc.) and the forces acting on it (such as driving force, braking force, friction, air resistance, etc.). Based on Newton's laws of motion or Lagrange's principles of mechanics, it considers 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 its rotational motion and the interaction between the vehicle and the ground, serving as the foundation for precise control, trajectory prediction, and vehicle performance analysis.

[0036] Step S203: Construct a safe driving corridor based at least on 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.

[0037] Specifically, a safe driving corridor is a virtual boundary designed in autonomous driving technology to ensure vehicle safety in complex environments. It is a strategically planned space defined as an area surrounding a desired path, whose size and shape are designed to prevent collisions with detected obstacles. The construction of a safe driving corridor comprehensively considers vehicle size, motion characteristics, obstacle locations, and potential uncertainties to ensure vehicle safety while driving within it.

[0038] Step S204: Determine whether the current position is within the safe driving corridor. If the current position is not within the safe driving corridor, use a dynamic window search control algorithm to determine the optimal path, and control the tracked vehicle 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 mobile robotics, especially in autonomous vehicles and Automated Guided Vehicles (AGVs). Its core idea is to dynamically select an optimal velocity control sequence within the context of the robot's current state (such as position, velocity, and acceleration) and its physical and kinematic constraints. This ensures the robot can safely and efficiently reach the target point while avoiding collisions with static or dynamic obstacles. A key advantage of the DWA algorithm is its ability to adjust the robot's behavior in real time in unknown or dynamically changing environments to cope with unexpected situations. It does not require prior knowledge of the entire environment or the precise location of obstacles; instead, it makes decisions within a local area. This makes DWA particularly suitable for scenarios where complete environment modeling is not possible or where 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. Therefore, appropriate optimization is required in practical applications based on specific scenarios.

[0040] Step S205: When the current position is within the safe driving corridor, the driving torque is calculated using the following path constraint algorithm, and the tracked vehicle is controlled to follow the desired path based on the driving torque.

[0041] Specifically, a path-following constraint algorithm is a control strategy that ensures a mobile robot or autonomous vehicle stays on a pre-defined path during movement. This algorithm is commonly used for path-following tasks, aiming to make the vehicle's actual trajectory as close as possible to a given desired path, while considering the vehicle's kinematic and dynamic constraints. The core of the path-following constraint algorithm lies in calculating vehicle control variables (such as speed, acceleration, steering angle, driving torque, etc.) to meet the position and orientation requirements on the path, while ensuring the vehicle's stability and safety. Driving torque is the force acting on the vehicle's drive shaft that propels the vehicle forward or backward. In tracked vehicles, driving torque is generated by the drive motor and transmitted to the tracks through the transmission system, causing the tracks to rotate and propelling the vehicle forward. Calculating driving torque typically involves the analysis of the vehicle's dynamics model, requiring consideration of many factors such as the vehicle's mass, inertia, the interaction between the tires or tracks and the ground, and the efficiency of the power system.

[0042] In this embodiment, firstly, the desired path of the tracked vehicle, its current position, its state, and the positions of obstacles are obtained. The desired path is a pre-set travel route for the tracked vehicle, and the state characterizes its motion characteristics. Then, at least based on the current position, a vehicle dynamics model is constructed, which characterizes the mechanical behavior and performance of the tracked vehicle during travel. Next, at least based on the desired path, current position, and obstacle positions, a safe travel corridor is constructed, which controls the tracked vehicle to avoid collisions with obstacles during path tracking. Finally, it is determined whether the current position is within the safe travel corridor. If the current position is not within the safe travel corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe travel corridor based on the optimal path. If the current position is within the safe travel corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to follow the desired path based on the driving torque. The path tracking control method for this tracked vehicle comprises at least three parts: a vehicle dynamics model, a safe driving corridor, and a path tracking control algorithm. The vehicle dynamics model fully incorporates the structural characteristics of the tracked vehicle, analyzes its mechanical properties, and constructs a kinematic and dynamic model. The safe driving corridor considers the safety of the tracked vehicle during the tracking process. The path tracking control algorithm includes two scenarios: a dynamic window search-based algorithm for the initial condition where the vehicle is outside the safe corridor, and a constraint-following-based algorithm for the vehicle within the safe corridor. These algorithms control the tracked vehicle's movement, enabling it to avoid obstacles and enter the safe corridor when initially outside, and accurately track the desired path within the safe corridor. This improves the safety and accuracy of path tracking control, meeting the requirements of actual operational scenarios. It solves the problem of insufficient safety and accuracy in the path tracking control of tracked vehicles in existing technologies.

[0043] In the specific implementation process, the above step S201 can be achieved through the following steps: Step S2011, obtain multiple target points, and connect the multiple target points sequentially to form the desired 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. The heading angle of the tracked vehicle is given; Step S2013: Obtain the linear velocity of the center of gravity 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; Step S2014: According to the formula: Determine the location P of the aforementioned obstacle.obstacle , where x obs and y obs Here are the position coordinates of the aforementioned obstacles in the geodetic coordinate system. The heading angle of the aforementioned obstacle is given. This method can further obtain the precise desired 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] To further accelerate the construction of the aforementioned vehicle dynamics model, step S202 of this application can be achieved through the following steps: Step S2021, according to the formula... Construct the Lagrange equation L for the tracked vehicle, where m is the mass of the tracked vehicle, J is the moment of inertia about the center of mass of the tracked vehicle, and ξ is the first state vector of the tracked vehicle. M is the inertia matrix of the tracked vehicle mentioned above; 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 a kinematic model of the tracked vehicle described above, where L is the distance between the left and right tracks of the tracked vehicle, b is the average track width of the tracked vehicle, and θ is the kinematic model of the tracked vehicle. L Let θ be the rotation angle of the left track of the aforementioned tracked vehicle. R Let ζ be the rotation angle of the right track of the tracked vehicle, and ζ be the second state vector of the tracked vehicle, ζ = [θ]. R ,θ L ] T , Step S2024: Substitute the above kinematic model into the above dynamic equations to obtain the above vehicle dynamic model. in, Where u is the driving torque of the tracked vehicle mentioned above.

[0045] Specifically, a vehicle dynamics model should be constructed based on at least the vehicle's current position. While position itself is not a direct element in constructing the dynamics model, current position information is crucial for vehicle control and state updates. Below are the basic steps for constructing a simple vehicle dynamics model, using a two-wheel-drive vehicle as an example: Define a coordinate system: First, a coordinate system needs to be defined. In most cases, a geodetic coordinate system or a vehicle-mounted coordinate system is chosen. The geodetic coordinate system typically uses north as the positive x-axis and east as the positive y-axis, while the vehicle-mounted coordinate system's x-axis points in the vehicle's forward direction and its y-axis points in the vehicle's width direction. Determine state variables: State variables are the set of parameters describing the vehicle's motion state, and should at least include position (x, y).

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

[0047] In practical applications, such as Figure 3 As shown, when the target points on the desired path are sufficiently dense, a circular safe area of ​​n target points can form a safe driving corridor. The path tracking control strategy controls the vehicle to follow the desired path within this safe corridor, thereby ensuring tracking safety. When the number of target points is not dense enough, the number of desired target points can be increased through interpolation and path smoothing.

[0048] In some embodiments, the process after step S203 and before step S204 can be specifically implemented through 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 ) represents the current position P mentioned above. veh With the i-th target point P i des The Euclidean distance between them, with each of the aforementioned target points as the center and R as the radius, does not include the aforementioned obstacles. The desired path includes multiple sequentially connected target points. x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system. The heading angle of the aforementioned tracked vehicle. and Let be the position coordinates of the i-th target point in the geodetic coordinate system. Let the heading angle be the i-th target point mentioned above; Step S207, according to the formula Calculate the target constraint e of the tracked vehicle mentioned above. target Step S208: When both the aforementioned safety constraint and the aforementioned target constraint approach 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 tracking error e in the safe area is... safezone and target point tracking error e target All values ​​approach 0, thus satisfying safety and target constraints, ensuring that the tracked vehicle tracks the target point P. i des Throughout the process, the vehicle remains within a safe zone and accurately tracks the target point. When tracking the next target point, the same constraint is satisfied, ensuring that the tracked vehicle always tracks the desired path within the defined safe driving corridor, thereby guaranteeing path tracking safety.

[0051] The above step S204 can be achieved through the following steps: Step S2041, determine the step, according to V sample ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max The sampling range V for the aforementioned tracked vehicles was determined. 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 v is the minimum linear velocity of the center of mass of the tracked vehicle mentioned above. max ω represents the maximum linear velocity of the center of mass of the tracked vehicle described above. min For the minimum angular velocity of the tracked vehicle mentioned above, ω max The maximum angular velocity of the tracked vehicle mentioned above; Step S2042, calculation step, according to the formula: Calculate the number of samples N samp Where Δv is the sampling velocity interval of the linear velocity of the center of mass of the tracked vehicle, and Δω is the sampling velocity interval of the angular velocity of the tracked vehicle; Step S2043, update step, according to the formula For the current position P mentioned above veh Update to obtain the update position P pre ,in, Δt is the sampling time interval between the linear velocity of the center of mass of the tracked vehicle and the angular velocity of the tracked vehicle; Step S2044, first repetition step, repeat the above update step multiple times, and update the current position to the updated position obtained in the previous repetition process during the repetition process, until a preset number of times is reached to obtain N. samp A preset trajectory; step S2045, calculation step, according to the formula E=g×D(P) j ,P obstacle )+h×(π-Δθ), calculate the evaluation function E for 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 ) represents the last position P in the aforementioned preset trajectory. j With the aforementioned obstacle P obstacle The Euclidean distance between the positions, Δθ is P j The tangent direction of the i-th target point P i des The angle between the lines connecting the points, the desired path includes a plurality of target points connected in sequence; Step S2046, determination step, determining the preset trajectory corresponding to the maximum value among the plurality of evaluation functions as the optimal trajectory, and obtaining the linear velocity and angular velocity of the tracked vehicle on the optimal trajectory; Step S2047, first control step, according to the formula Calculate the speed v of the left track of the tracked vehicle described above. L And control the left track of the aforementioned tracked vehicle to travel at the aforementioned left track speed, wherein v opt Let L be the linear velocity of the tracked vehicle on the optimal trajectory, L be the distance between the left and right tracks of the tracked vehicle, b be the average track width of the tracked vehicle, and ω be the linear velocity of the tracked vehicle on the optimal trajectory. opt The angular velocity of the tracked vehicle on the optimal trajectory; Step S2048, the second control step, according to the formula Calculate the speed v of the right track of the tracked vehicle described above. R The method controls the right track of the tracked vehicle to travel at the specified right track speed; step S2049, a second repeating step, involves sequentially repeating the above-mentioned determining step, calculation step, update step, first repeating step, calculation step, determining step, first control step, and second control step at least once, until the tracked vehicle enters the safe driving corridor. This method can further enable the tracked vehicle to quickly enter the safe driving corridor.

[0052] The above step S205 can be achieved through 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, and ζ = [θ R ,θ L ] T θ L Let θ be the rotation angle of the left track of the aforementioned tracked vehicle. R This refers to the rotation angle of the right track of the aforementioned tracked vehicle. C is the Coriolis centrifugal force of the tracked vehicle, and κ is an adjustable coefficient greater than 0; in step S2052, the driving torque is substituted into the vehicle dynamics model to obtain the first target linear velocity and the second target linear velocity, and the left track of the tracked vehicle is controlled to travel at the first target linear velocity, and the right track of the tracked vehicle is controlled to travel at the second target linear velocity.

[0053] Specifically, the steps for calculating the driving torque are as follows: First, construct a mathematical model based on the vehicle's dynamic characteristics, as mentioned above, using Lagrange's equations or Newton's laws of motion. The model should include vehicle state variables (such as position, velocity, and acceleration) and various forces and torques acting on the vehicle (such as driving torque, friction, and air resistance). Real-time acquisition of the vehicle's current position, velocity, acceleration, steering angle, and other state information is typically achieved through onboard sensors and a control system. Based on the vehicle's current state and the desired path, calculate the deviation between the vehicle and the path. This includes lateral and longitudinal deviations, as well as the deviation between the vehicle's direction and the path direction. Design a controller capable of outputting the required control input based on the path deviation and vehicle state. In path tracking control, the controller's goal is to adjust the driving torque so that the vehicle can travel along the desired path while satisfying dynamic constraints such as maximum acceleration and maximum speed. Using the designed controller algorithm, calculate the required driving torque for the left and right tracks based on the vehicle's current state and path deviation. This may involve feedback control, predictive control, or model-based control techniques such as PID control and Model Predictive Control (MPC). During vehicle operation, the above steps are repeated continuously, and the driving torque is dynamically adjusted based on real-time path deviation and vehicle status to ensure the vehicle continuously travels along the desired path. When calculating the driving torque, safety constraints must be considered, such as avoiding collisions with obstacles and staying within the safe corridor. This is typically achieved by adding additional safety constraints or obstacle avoidance algorithms to the controller.

[0054] To enable those skilled in the art to better understand the technical solution of this application, the implementation process of the path tracking control method for tracked vehicles of this application will be described in detail below with reference to specific embodiments.

[0055] This embodiment relates to a specific path tracking control method for tracked vehicles, such as... Figure 4As shown, it includes the following steps:

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

[0057] Step S2: Construct the vehicle dynamics model, safe driving corridor, and equality constraints;

[0058] Step S3: Determine whether the vehicle is located within the safe corridor under the initial conditions. If not, proceed to step S4; otherwise, proceed to step S5.

[0059] Step S4: Execute the path planning method based on dynamic window search, output the lateral speeds of the left and right tracks, and continuously update the vehicle position;

[0060] Step S5: Execute the constraint-based path planning method, output the driving torque, and control the vehicle to accurately track the desired path within the safe corridor.

[0061] This application also provides a path tracking control device for tracked vehicles. It should be noted that the path tracking control device for tracked vehicles in this application can be used to execute the path tracking control method for tracked vehicles provided in this application. This device is used to implement the above embodiments and preferred embodiments; details already described will not be repeated. As used below, the term "module" can refer to a combination of software and / or hardware that performs a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0062] The path tracking control device for tracked vehicles provided in the embodiments of this application will be described below.

[0063] Figure 5 This is a schematic diagram of a path tracking control device for a tracked vehicle according to an embodiment of this application. Figure 5 As shown, the device includes:

[0064] The acquisition unit 10 is used to acquire the desired path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of the obstacle. The desired path is a pre-set travel 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 an autonomous driving system is responsible for generating an optimal path from the vehicle's starting point to its destination, based on the vehicle's origin, destination, and real-time environmental information. The desired path output by the path planning layer includes a series of target points, each containing coordinate information in the vehicle's coordinate system. In certain specific environments, such as mining or agricultural scenarios, paths may be pre-set and stored in the vehicle's control unit. These pre-set paths can be based on historical data, environmental mapping, or manually defined. The tracked vehicle's current position and status information can be obtained through a Global Positioning System (GPS), Inertial Measurement Unit (IMU), wheel speed sensors, and vision sensors. Obstacle locations can be obtained through lidar, ultrasonic sensors, radar, and vision sensors.

[0066] The first building unit 20 is used to build 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.

[0067] Specifically, a vehicle dynamics model is a mathematical model that describes the relationship between a vehicle's motion (such as velocity, acceleration, angular velocity, angular acceleration, etc.) and the forces acting on it (such as driving force, braking force, friction, air resistance, etc.). Based on Newton's laws of motion or Lagrange's principles of mechanics, it considers 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 its rotational motion and the interaction between the vehicle and the ground, serving as the foundation for precise control, trajectory prediction, and vehicle performance analysis.

[0068] The second construction unit 30 is used to construct a safe driving corridor based at least on 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.

[0069] Specifically, a safe driving corridor is a virtual boundary designed in autonomous driving technology to ensure vehicle safety in complex environments. It is a strategically planned space defined as an area surrounding a desired path, whose size and shape are designed to prevent collisions with detected obstacles. The construction of a safe driving corridor comprehensively considers vehicle size, motion characteristics, obstacle locations, and potential uncertainties to ensure vehicle safety while driving within it.

[0070] The first control unit 40 is used to determine 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.

[0071] Specifically, the Dynamic Window Approach (DWA) is a local path planning and obstacle avoidance algorithm widely used in mobile robotics, especially in autonomous vehicles and Automated Guided Vehicles (AGVs). Its core idea is to dynamically select an optimal velocity control sequence within the context of the robot's current state (such as position, velocity, and acceleration) and its physical and kinematic constraints. This ensures the robot can safely and efficiently reach the target point while avoiding collisions with static or dynamic obstacles. A key advantage of the DWA algorithm is its ability to adjust the robot's behavior in real time in unknown or dynamically changing environments to cope with unexpected situations. It does not require prior knowledge of the entire environment or the precise location of obstacles; instead, it makes decisions within a local area. This makes DWA particularly suitable for scenarios where complete environment modeling is not possible or where 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. Therefore, appropriate optimization is required in practical applications based on specific scenarios.

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

[0073] Specifically, a path-following constraint algorithm is a control strategy that ensures a mobile robot or autonomous vehicle stays on a pre-defined path during movement. This algorithm is commonly used for path-following tasks, aiming to make the vehicle's actual trajectory as close as possible to a given desired path, while considering the vehicle's kinematic and dynamic constraints. The core of the path-following constraint algorithm lies in calculating vehicle control variables (such as speed, acceleration, steering angle, driving torque, etc.) to meet the position and orientation requirements on the path, while ensuring the vehicle's stability and safety. Driving torque is the force acting on the vehicle's drive shaft that propels the vehicle forward or backward. In tracked vehicles, driving torque is generated by the drive motor and transmitted to the tracks through the transmission system, causing the tracks to rotate and propelling the vehicle forward. Calculating driving torque typically involves the analysis of the vehicle's dynamics model, requiring consideration of many factors such as the vehicle's mass, inertia, the interaction between the tires or tracks and the ground, and the efficiency of the power system.

[0074] In this embodiment, the acquisition unit acquires the desired path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of obstacles. The desired path is a pre-set travel route for the tracked vehicle, and the state characterizes the motion characteristics of the tracked vehicle. The first construction unit constructs a vehicle dynamics model based at least on the current position. This model characterizes the mechanical behavior and performance of the tracked vehicle during travel. The second construction unit then constructs a safe travel corridor based at least on the desired path, the current position, and the positions of obstacles. This safe travel corridor controls the tracked vehicle to avoid collisions with obstacles during path tracking. The first control unit determines whether the current position is within the safe travel corridor. If the current position is not within the safe travel corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe travel corridor based on the optimal path. If the current position is within the safe travel corridor, the second control unit uses a following path constraint algorithm to calculate the driving torque and controls the tracked vehicle to follow the desired path based on the driving torque. The path tracking control method for this tracked vehicle comprises at least three parts: a vehicle dynamics model, a safe driving corridor, and a path tracking control algorithm. The vehicle dynamics model fully incorporates the structural characteristics of the tracked vehicle, analyzes its mechanical properties, and constructs a kinematic and dynamic model. The safe driving corridor considers the safety of the tracked vehicle during the tracking process. The path tracking control algorithm includes two scenarios: a dynamic window search-based algorithm for the initial condition where the vehicle is outside the safe corridor, and a constraint-following-based algorithm for the vehicle within the safe corridor. These algorithms control the tracked vehicle's movement, enabling it to avoid obstacles and enter the safe corridor when initially outside, and accurately track the desired path within the safe corridor. This improves the safety and accuracy of path tracking control, meeting the requirements of actual operational scenarios. It solves the problem of insufficient safety and accuracy in the path tracking control of tracked vehicles in existing technologies.

[0075] In specific implementation, the aforementioned acquisition unit includes a first acquisition module, a first determination module, a second acquisition module, and a second determination module. The first acquisition module is used to acquire multiple target points, which are then connected sequentially to form the desired path. The first determination module is used to determine the 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. The first module is for obtaining the heading angle of the tracked vehicle; the second module is for obtaining the linear velocity of the center of gravity 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 module is for determining the state according to the formula. Determine the location P of the aforementioned obstacle. obstacle , where x obs and y obs Here are the position coordinates of the aforementioned obstacles in the geodetic coordinate system. The heading angle of the aforementioned obstacle. This device can further obtain the precise desired 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] To further accelerate the construction of the aforementioned vehicle dynamics model, the first construction unit of this 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 construct the vehicle dynamics model according to the formula. Construct the Lagrange equation L for the tracked vehicle, where m is the mass of the tracked vehicle, J is the moment of inertia about the center of mass of the tracked vehicle, and ξ is the first state vector of the tracked vehicle. M is the inertia matrix of the tracked vehicle mentioned above; the second building module is used to calculate according to the formula The dynamic equation u(t) of the tracked vehicle is constructed, where C is the Coriolis centrifugal force of the tracked vehicle; the third construction module is used to construct the equation u(t) according to the formula. Construct a kinematic model of the tracked vehicle described above, where L is the distance between the left and right tracks of the tracked vehicle, b is the average track width of the tracked vehicle, and θ is the kinematic model of the tracked vehicle. L Let θ be the rotation angle of the left track of the aforementioned tracked vehicle. R Let ζ be the rotation angle of the right track of the tracked vehicle, and ζ be the second state vector of the tracked vehicle, ζ = [θ]. R ,θ L ] T , The first substitution module is used to substitute the above kinematic model into the above dynamic equations to obtain the above vehicle dynamic model. in, Where u is the driving torque of the tracked vehicle mentioned above.

[0077] Specifically, a vehicle dynamics model should be constructed based on at least the vehicle's current position. While position itself is not a direct element in constructing the dynamics model, current position information is crucial for vehicle control and state updates. Below are the basic steps for constructing a simple vehicle dynamics model, using a two-wheel-drive vehicle as an example: Define a coordinate system: First, a coordinate system needs to be defined. In most cases, a geodetic coordinate system or a vehicle-mounted coordinate system is chosen. The geodetic coordinate system typically uses north as the positive x-axis and east as the positive y-axis, while the vehicle-mounted coordinate system's x-axis points in the vehicle's forward direction and its y-axis points in the vehicle's width direction. Determine state variables: State variables are the set of parameters describing the vehicle's motion state, and should at least include position (x, y).

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

[0079] In practical applications, such as Figure 3 As shown, when the target points on the desired path are sufficiently dense, a circular safe area of ​​n target points can form a safe driving corridor. The path tracking control strategy controls the vehicle to follow the desired path within this safe corridor, thereby ensuring tracking safety. When the number of target points is not dense enough, the number of desired target points can be increased through interpolation and path smoothing.

[0080] In some embodiments, the above-described apparatus further includes a first computing unit, a second computing unit, and a processing unit, wherein the first computing unit is used to process the data 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 ) represents the current position P mentioned above. veh With the i-th target point Pi des The Euclidean distance between them, with each of the aforementioned target points as the center and R as the radius, does not include the aforementioned obstacles. The desired path includes multiple sequentially connected target points. x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system. The heading angle of the aforementioned tracked vehicle. and Let be the position coordinates of the i-th target point in the geodetic coordinate system. Let be the heading angle of the i-th target point mentioned above; the second calculation unit is used to calculate according to the formula: Calculate the target constraint e of the tracked vehicle mentioned above. target The processing unit is configured to ensure that, when both the aforementioned safety constraints and the aforementioned target constraints approach 0, the first state vector ξ of the tracked vehicle satisfies... in, This device can further enhance the security of path tracking.

[0081] Specifically, when the above first-order and second-order equality constraints are satisfied, the tracking error e in the safe area is... sa f ezone and target point tracking error e target All values ​​approach 0, thus satisfying safety and target constraints, ensuring that the tracked vehicle tracks the target point P. i des Throughout the process, the vehicle remains within a safe zone and accurately tracks the target point. When tracking the next target point, the same constraint is satisfied, ensuring that the tracked vehicle always tracks the desired path within the defined safe driving corridor, thereby guaranteeing path tracking safety.

[0082] The aforementioned first control unit includes: 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 steps, based on V. sample ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max The sampling range V for the aforementioned tracked vehicles was determined. 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 v is the minimum linear velocity of the center of mass of the tracked vehicle mentioned above. max ω represents the maximum linear velocity of the center of mass of the tracked vehicle described above. min For the minimum angular velocity of the tracked vehicle mentioned above, ωmax The maximum angular velocity of the tracked vehicle mentioned above; the second calculation module is used for calculation steps, according to the formula. Calculate the number of samples N samp Where Δv is the sampling velocity interval of the linear velocity of the center of mass of the tracked vehicle, and Δω is the sampling velocity interval of the angular velocity of the tracked vehicle; the update module is used for the update step, according to the formula For the current position P mentioned above veh Update to obtain the update position P pre ,in, Δt is the sampling time interval between the linear velocity of the center of gravity 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 above update step multiple times, and updating the current position to the updated position obtained in the previous repetition process during the repetition process, until a preset number of times is reached to obtain N. s a m A preset trajectory; 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 for 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 ) represents the last position P in the aforementioned preset trajectory. j With the aforementioned obstacle P obstacle The Euclidean distance between the positions, Δθ is P j The tangent direction of the i-th target point P i des The angle between the lines connecting the points, the desired path includes multiple target points connected in sequence; the fourth determining module is used in the determining step to determine the preset trajectory corresponding to the maximum value among the multiple evaluation functions as the optimal trajectory, and to obtain the linear velocity and angular velocity of the tracked vehicle on the optimal trajectory; the first control module is used in the first control step, according to the formula Calculate the speed v of the left track of the tracked vehicle described above. L And control the left track of the aforementioned tracked vehicle to travel at the aforementioned left track speed, wherein v opt Let L be the linear velocity of the tracked vehicle on the optimal trajectory, L be the distance between the left and right tracks of the tracked vehicle, b be the average track width of the tracked vehicle, and ω be the linear velocity of the tracked vehicle on the optimal trajectory. opt The angular velocity of the tracked vehicle on the optimal trajectory is given; the second control module is used for the second control step, according to the formula... Calculate the speed v of the right track of the tracked vehicle described above. RThe device controls the right track of the tracked vehicle to travel at the specified right track speed. A second repeat module is used for the second repeat step, sequentially repeating the determination step, calculation step, update step, first repeat step, calculation step, determination step, first control step, and second control step at least once, until the tracked vehicle enters the safe driving corridor. This device can further enable the tracked vehicle to quickly enter the safe driving corridor.

[0083] The aforementioned second control unit includes a fourth calculation module and a second input module, wherein the fourth calculation module is used to calculate 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, and ζ = [θ R ,θ L ] T θ L Let θ be the rotation angle of the left track of the aforementioned tracked vehicle. R This refers to the rotation angle of the right track of the aforementioned tracked vehicle. C is the Coriolis centrifugal force of the tracked vehicle, and κ is an adjustable coefficient greater than 0; the second input module is used to input the driving torque into the vehicle dynamics model to obtain the first target linear velocity and the second target linear velocity, and control the left track of the tracked vehicle to travel at the first target linear velocity, and control the right track of the tracked vehicle to travel at the second target linear velocity.

[0084] Specifically, the steps for calculating the driving torque are as follows: First, construct a mathematical model based on the vehicle's dynamic characteristics, as mentioned above, using Lagrange's equations or Newton's laws of motion. The model should include vehicle state variables (such as position, velocity, and acceleration) and various forces and torques acting on the vehicle (such as driving torque, friction, and air resistance). Real-time acquisition of the vehicle's current position, velocity, acceleration, steering angle, and other state information is typically achieved through onboard sensors and a control system. Based on the vehicle's current state and the desired path, calculate the deviation between the vehicle and the path. This includes lateral and longitudinal deviations, as well as the deviation between the vehicle's direction and the path direction. Design a controller capable of outputting the required control input based on the path deviation and vehicle state. In path tracking control, the controller's goal is to adjust the driving torque so that the vehicle can travel along the desired path while satisfying dynamic constraints such as maximum acceleration and maximum speed. Using the designed controller algorithm, calculate the required driving torque for the left and right tracks based on the vehicle's current state and path deviation. This may involve feedback control, predictive control, or model-based control techniques such as PID control and Model Predictive Control (MPC). During vehicle operation, the above steps are repeated continuously, and the driving torque is dynamically adjusted based on real-time path deviation and vehicle status to ensure the vehicle continuously travels along the desired path. When calculating the driving torque, safety constraints must be considered, such as avoiding collisions with obstacles and staying within the safe corridor. This is typically 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, first construction unit, second construction unit, first control unit, and second control unit are all stored as program units in the memory. The processor executes these program units stored in the memory to achieve the corresponding functions. All of the above modules reside in the same processor; alternatively, the modules may be located in different processors in any combination.

[0086] The memory may include non-permanent memory in computer-readable media, such as 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] This invention provides a computer-readable storage medium including a stored program, wherein, when the program is executed, it controls the device containing the computer-readable storage medium to perform the path tracking control method for the tracked vehicle.

[0088] It is obvious to those skilled in the art that the modules or steps of the present invention described above can be implemented using general-purpose computing devices. They can be centralized on a single computing device or distributed across a network of multiple computing devices. They can be implemented using computer-executable program code, and thus can be stored in a storage device for execution by a computing device. In some cases, the steps shown or described can be performed in a different order than those described herein, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. Thus, the present invention is not limited to any particular combination of hardware and software.

[0089] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0090] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0091] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0092] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0093] Memory may include non-persistent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, like read-only memory (ROM) or flash RAM. Memory is an example of computer-readable media.

[0094] Computer-readable media includes both permanent and non-permanent, removable and non-removable media that can store information using any method or technology. Information can be computer-readable instructions, data structures, modules of programs, 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 technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other non-transferable medium that can be used to store information accessible by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0095] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0096] As can be seen from the above description, the embodiments of this application achieve the following technical effects:

[0097] 1) The path tracking control method for tracked vehicles of this application firstly obtains the desired 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 desired path is a pre-set travel route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle; then, at least based on 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 travel; next, at least based on the desired path, the current position, and the position of obstacles, a safe travel corridor is constructed, wherein the safe travel corridor is used to control the tracked vehicle to avoid collisions with obstacles during path tracking; finally, it is determined whether the current position is within the safe travel corridor. If the current position is not within the safe travel corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe travel corridor according to the optimal path; if the current position is within the safe travel corridor, a following path constraint algorithm is used to calculate the driving torque, and the tracked vehicle is controlled to follow the desired path according to the driving torque. The path tracking control method for this tracked vehicle comprises at least three parts: a vehicle dynamics model, a safe driving corridor, and a path tracking control algorithm. The vehicle dynamics model fully incorporates the structural characteristics of the tracked vehicle, analyzes its mechanical properties, and constructs a kinematic and dynamic model. The safe driving corridor considers the safety of the tracked vehicle during the tracking process. The path tracking control algorithm includes two scenarios: a dynamic window search-based algorithm for the initial condition where the vehicle is outside the safe corridor, and a constraint-following-based algorithm for the vehicle within the safe corridor. These algorithms control the tracked vehicle's movement, enabling it to avoid obstacles and enter the safe corridor when initially outside, and accurately track the desired path within the safe corridor. This improves the safety and accuracy of path tracking control, meeting the requirements of actual operational scenarios. It solves the problem of insufficient safety and accuracy in the path tracking control of tracked vehicles in existing technologies.

[0098] 2) The path tracking control device for tracked vehicles of this application includes an acquisition unit that acquires the desired path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of obstacles. The desired path is a pre-set travel route for the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle. A first construction unit constructs a vehicle dynamics model based at least on the current position. The vehicle dynamics model is used to characterize the mechanical behavior and performance of the tracked vehicle during travel. A second construction unit then constructs a safe travel corridor based at least on the desired path, the current position, and the position of obstacles. The safe travel corridor is used to control the tracked vehicle to avoid collisions with obstacles during path tracking. A first control unit determines whether the current position is within the safe travel corridor. If the current position is not within the safe travel corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe travel corridor based on the optimal path. If the current position is within the safe travel corridor, the second control unit uses a following path constraint algorithm to calculate the driving torque, and the tracked vehicle is controlled to follow the desired path based on the driving torque. The path tracking control method for this tracked vehicle comprises at least three parts: a vehicle dynamics model, a safe driving corridor, and a path tracking control algorithm. The vehicle dynamics model fully incorporates the structural characteristics of the tracked vehicle, analyzes its mechanical properties, and constructs a kinematic and dynamic model. The safe driving corridor considers the safety of the tracked vehicle during the tracking process. The path tracking control algorithm includes two scenarios: a dynamic window search-based algorithm for the initial condition where the vehicle is outside the safe corridor, and a constraint-following-based algorithm for the vehicle within the safe corridor. These algorithms control the tracked vehicle's movement, enabling it to avoid obstacles and enter the safe corridor when initially outside, and accurately track the desired path within the safe corridor. This improves the safety and accuracy of path tracking control, meeting the requirements of actual operational scenarios. It solves the problem of insufficient safety and accuracy in the path tracking control of tracked vehicles in existing technologies.

[0099] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A path tracking control method for a tracked vehicle, characterized in that, include: The desired 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 desired path is a pre-set travel route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle. Based at least on 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 travel; A safe driving corridor is constructed based at least on 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 collisions with the obstacle during path tracking; Determine whether the current position is within the safe driving corridor. If the current position is not within the safe driving corridor, use a dynamic window search control algorithm to determine the optimal path, and control 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 follow the desired path based on the driving torque.

2. The method according to claim 1, characterized in that, Obtaining the desired path of the tracked vehicle, the current position of the tracked vehicle, the state of the tracked vehicle, and the position of obstacles includes: Multiple target points are obtained, and the multiple target points are connected sequentially to form 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. Let be the heading angle of the tracked vehicle; The state of the tracked vehicle is obtained by acquiring the linear velocity of its center of gravity, 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. According to the formula Determine the position P of the obstacle. obstacle , where x obs and y obs The coordinates of the obstacle in the geodetic coordinate system are: 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 Construct the Lagrange equation L for the tracked vehicle a Where m is the mass of the tracked vehicle, J is the moment of inertia about the center of mass of the tracked vehicle, and ξ 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 and right tracks of the tracked vehicle, W is the average track width of the tracked vehicle, and θ... L Let θ be the rotation angle of the left track of the tracked vehicle. R Let ζ be the rotation angle of the right track of the tracked vehicle, and ζ be the second state vector of the tracked vehicle, ζ = [θ]. R θ L ] T , Substituting the kinematic model into the dynamic equation, we obtain the vehicle dynamic model. in, Where u is the current driving torque of the tracked vehicle.

4. The method according to claim 1, characterized in that, Construct a safe driving corridor based at least on the desired path, the current location, and the location of the obstacle, including: According to the formula Calculate the safe area Wherein, D(P) i des P veh ) represents the current position P veh With the i-th target point P i des The Euclidean distance between them, with each of the target points as the center and R as the radius, does not include the obstacles in the circle, and the desired path includes a plurality of the target points connected in sequence; According to the formula Construct the SafeZone, where n is the number of target points.

5. The method according to claim 1, characterized in that, After constructing a safe driving corridor based at least on the desired path, the current position, and the position of the obstacle, and before determining whether the current position is 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 ) represents the current position P veh With the i-th target point P i des The Euclidean distance between them, with each target point as the center and R as the radius, does not include the obstacles in the circle. The desired path includes multiple target points connected sequentially. x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system. Let be the heading angle of the tracked vehicle. and Let be the position coordinates of the i-th target point in the geodetic coordinate system. Let be the heading angle of the i-th target point; According to the formula: Calculate the target constraint e of the tracked vehicle target ; When both the safety constraint and the target constraint approach 0, the first state vector ξ of the tracked vehicle satisfies in, 6. The method according to claim 1, characterized in that, If the current position is not within the safe travel corridor, a dynamic window search control algorithm is used to determine the optimal path, and the tracked vehicle is controlled to enter the safe travel corridor according to the optimal path, including: Determine the steps, based on 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 v is the minimum linear velocity of the center of mass of the tracked vehicle. max ω is the maximum linear velocity of the center of mass of the tracked vehicle. min ω is the minimum angular velocity of the tracked vehicle. max This represents the maximum angular velocity of the tracked vehicle. Calculation steps, based on the formula: Calculate the number of samples N samp Where Δv is the sampling speed interval of the center-of-gravity linear velocity of the tracked vehicle, and Δω is the sampling speed interval of the angular velocity of the tracked vehicle. Update steps, according to the formula For the current position P veh Update to obtain the update position P pre ,in, Δt is the sampling time interval between the linear velocity of the tracked vehicle's center of mass and the angular velocity of the tracked vehicle, and x and y are the position coordinates of the tracked vehicle in the geodetic coordinate system. Let be the heading angle of the tracked vehicle; The first repetition step involves repeating the update step multiple times, updating the current position to the updated position obtained in the previous repetition during each repetition, until a preset number of repetitions is reached, resulting in N. samp A preset trajectory; Calculation steps, according to the formula E=g×D(P) j P obstacle )+h×(π-Δθ), calculate the evaluation function E for each of the preset trajectories, where g and h are weight coefficients, both g and h are greater than 0, D(P j P obstacle ) represents 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 the i-th target point P i des The angle between the lines connecting the target points, and the desired path includes a plurality of target points connected in sequence; The steps include determining the preset trajectory corresponding to the maximum value among multiple evaluation functions as the optimal trajectory, and obtaining the linear velocity and angular velocity of the tracked vehicle on the optimal trajectory. The first control step, according to the formula Calculate the speed v of the left track of the tracked vehicle. L And control the left track of the tracked vehicle to travel at the speed of the left track, where v opt Let L be the linear velocity of the tracked vehicle on the optimal trajectory, L be the distance between the left and right tracks of the tracked vehicle, W be the average track width of the tracked vehicle, and ω be the linear velocity of the tracked vehicle on the optimal trajectory. opt The angular velocity of the tracked vehicle on the optimal trajectory; The second control step, according to the formula... Calculate the speed v of the right track of the tracked vehicle. R And control the right track of the tracked vehicle to travel at the speed of the right track; The second repeating step involves sequentially repeating the determining step, the calculation step, the updating step, the first repeating step, the calculation 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.

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 follow the desired path based on 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, and ζ = [θ R θ L ] T θ L Let θ be the rotation angle of the left track of the tracked vehicle. R Let be 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 input into the vehicle dynamics model to obtain a first target linear velocity and a second target linear velocity. The left track of the tracked vehicle is controlled to travel at the first target linear velocity, and the right track of the tracked vehicle is controlled to travel at the second target linear velocity.

8. A path tracking control device for a tracked vehicle, characterized in that, include: The acquisition unit is used to acquire the desired 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 desired path is a pre-set travel route of the tracked vehicle, and the state is used to characterize the motion characteristics of the tracked vehicle. The first construction unit is configured to construct 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 operation; The second construction unit is used to construct a safe driving corridor based at least on 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. The first control unit is used to determine 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. The second control unit is used to calculate the driving torque using a following path constraint algorithm when the current position is within the safe driving corridor, and to control the tracked vehicle to follow 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, it controls the device on which the computer-readable storage medium is located to perform 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, it implements the steps of the method described in any one of claims 1 to 7.

Images