Dynamic obstacle avoidance decision control method for unmanned tracked vehicle in field environment

Abstract

The invention relates to the technical field of unmanned tracked vehicle obstacle avoidance decision control, in particular to an unmanned tracked vehicle dynamic obstacle avoidance decision control method in a field environment, which comprises the following steps: obtaining an expected global path and an expected global speed of a tracked vehicle; acquiring the motion states of the crawler and the obstacle at the current moment; constructing a trajectory tracking and obstacle avoidance control multi-constraint optimization control problem by using model predictive control; a dynamic obstacle avoidance decision strategy trial and error mechanism is constructed, and an expected obstacle avoidance decision instruction and an expected obstacle avoidance motion control instruction of the crawler are obtained; constructing a crawler motion execution controller to track the expected obstacle avoidance motion control instruction to obtain a crawler motion execution control instruction; and executing the control instruction based on the crawler motion to control the vehicle to move. According to the method, the multi-dimensional vehicle kinematics constraint and the dynamic obstacle safety constraint are introduced, and the efficient and safe unmanned tracked vehicle trajectory tracking and collision avoidance capacity is achieved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of unmanned tracked vehicle obstacle avoidance decision control, and particularly relates to a dynamic obstacle avoidance decision control method for unmanned tracked vehicles in a field environment. BACKGROUND

[0002] Decision and control technology is the "brain" and "hands and feet" of an unmanned tracked vehicle, directly determines the motion behavior of the vehicle, and has a significant impact on the environmental adaptability and safety of the vehicle. However, the existing decision control method does not consider the kinematic constraints and dynamic obstacle constraints of the tracked vehicle in the motion process, and due to insufficient consideration of environmental prediction information, the ability to deal with dynamic time-varying obstacles is limited, and it is difficult to ensure high safety dynamic collision avoidance while tracking the trajectory.

[0003] Therefore, how to realize efficient, safe and adaptive dynamic obstacle avoidance decision control of unmanned tracked vehicles on the basis of considering trajectory tracking multi-constraints and obstacle prediction information is an important challenge. SUMMARY

[0004] The application aims to solve the problem that the existing technology has limited ability to deal with dynamic time-varying obstacles when controlling the vehicle, and proposes a dynamic obstacle avoidance decision control method for unmanned tracked vehicles in a field environment.

[0005] The application provides a dynamic obstacle avoidance decision control method for unmanned tracked vehicles in a field environment, which comprises the following steps:

[0006] S1: receiving output information of a global path planning and speed planning module in an unmanned driving system of the tracked vehicle, to obtain a global path and a global speed expected by the tracked vehicle;

[0007] S2: collecting the motion state of the tracked vehicle and obstacles at the current time based on a vehicle-mounted sensor;

[0008] S3: based on the motion state of the tracked vehicle at the current time and the global path and global speed expected by the tracked vehicle obtained in step S1, constructing a trajectory tracking and obstacle avoidance control multi-constraint optimization control problem by using model predictive control;

[0009] S4: based on the trajectory tracking and obstacle avoidance control multi-constraint optimization control problem and the motion state of the obstacles at the current time obtained in step S2, constructing a dynamic obstacle avoidance decision strategy trial-and-error mechanism to obtain an obstacle avoidance decision instruction and an obstacle avoidance motion control instruction expected by the tracked vehicle;

[0010] S5: constructing a tracked vehicle motion execution controller to track the obstacle avoidance motion control instruction expected by the tracked vehicle, to obtain a tracked vehicle motion execution control instruction;

[0011] S6: execute the control instruction based on the tracked vehicle motion to control the vehicle motion.

[0012] The step S2 specifically comprises:

[0013] S2-1: collect the motion state of the tracked vehicle and the obstacle at the current time, the motion state comprising: global coordinates (X, Y) of the vehicle mass center in the earth coordinate system XOY; longitudinal velocity v x and lateral velocity v y of the vehicle mass center in the vehicle body coordinate system xoy; yaw rate r of the vehicle at the mass center; heading angle θ of the vehicle; longitudinal acceleration a x of the vehicle mass center in the vehicle body coordinate system; yaw angular acceleration r d of the vehicle at the mass center; relative longitudinal distance Δx, relative lateral distance Δy, relative longitudinal velocity Δv x , and relative lateral velocity Δv y of the vehicle and the obstacle in the vehicle body coordinate system.

[0014] The step S3 specifically comprises:

[0015] S3-1: based on the expected global path of the tracked vehicle obtained in step S1, project the global coordinates of the tracked vehicle at the current time onto the expected global path to obtain a tracked vehicle global path projection point;

[0016] S3-2: take the tracked vehicle global path projection point as the origin, sample N1 times along the expected global path at a sampling time interval Δt and the expected global speed of the tracked vehicle obtained in step S1, and project the sampling points to the tracked vehicle body coordinate system to obtain tracked vehicle reference trajectory points (x ref,k , y ref,k , θ ref,k ), wherein the three coordinates are respectively the longitudinal position, lateral position, and heading angle of the kth expected trajectory point;

[0017] S3-3: construct a tracked vehicle motion control differential model:

[0018]

[0019] wherein, are the first-order derivatives of v x , v y , and r; κ is the side slip friction coefficient of the tracked vehicle; g is the gravitational acceleration; tanh is the hyperbolic tangent function; σ is a small real number set; and l is the longitudinal length of the tracked vehicle ground footprint.

[0020] Construct a tracked vehicle kinematics equation:

[0021]

[0022] where x, y are the longitudinal and lateral positions of the vehicle mass center in the vehicle body coordinate system, respectively; are the first-order derivatives of x, y, θ, respectively;

[0023] S3-4: Define the tracked vehicle control state space model state variables Output variables and control variables

[0024]

[0025] S3-5: Perform Taylor expansion at the reference working point to build a linearized time-discrete prediction model:

[0026]

[0027] where k is the prediction time step index; is the state variable at time k; are the state variable and output variable at time k+1, respectively; is the control variable at time k; I is the identity matrix; F(·) represents the differential model obtained by combining and augmenting the tracked vehicle motion control differential model and kinematics equation built in step S3-3; is the partial differential symbol; is the reference working point; Γ is the state variable to output variable conversion matrix;

[0028] S3-6: Construct a tracked vehicle trajectory tracking optimization control problem:

[0029]

[0030] where J is the total optimization objective; ΔU is the control variable increment sequence to be optimized, defined as follows:

[0031]

[0032] where Δu n = u n - u n-1 is the difference between the nth control variable u n and the (n-1)th control variable u n-1 ; N2 is the total number of control time steps; in addition, J track is the trajectory tracking accuracy optimization objective, J u and J Δu are the smoothness optimization objectives in the trajectory tracking process by suppressing the amplitude of the control variable and by reducing the rate of change of the control variable, respectively, which are defined as follows:

[0033]

[0034] in, is the output variable at time k; is the expected trajectory point vector of the tracked vehicle; Q, R u 、R Δu are the weight matrices corresponding to the three optimization objectives respectively;

[0035] Construct the trajectory tracking and obstacle avoidance control optimization problem constraints:

[0036]

[0037] Among them, Γ x , Γ u , Γ Δu are matrices for transforming the state variables, control variables, and components of the control variable change rate vector that need to be constrained; are the lower and upper limits of the components in the state variables that need to be constrained; is the lower and upper limits of the components that need to be constrained in the control variables; are the lower and upper limits of the components in the control variable change rate vector that need to be constrained.

[0038] The step S4 specifically includes:

[0039] S4-1: Based on the expected global path of the tracked vehicle obtained in step S1, a lateral motion deviation boundary of the tracked vehicle during the trajectory tracking process is constructed;

[0040] S4-2: Based on the current motion state of the obstacle obtained in step S2, the obstacle motion is predicted to obtain the position prediction sequence points (x obs,k ,y obs,k ), and construct the obstacle envelope to predict the motion boundary;

[0041] S4-3: Based on the lateral motion deviation boundary of the tracked vehicle during the trajectory tracking process obtained in step S4-1 and the position prediction sequence points of the obstacle in the model predictive control prediction time domain, the obstacle envelope position prediction sequence point numbers within the lateral motion deviation boundary and intersecting with the boundary are calculated and screened, and the obstacle prediction position corresponding to the first and last sequence point numbers is defined as (x obs,k0 ,y obs,k0 ) and (x obs,k0+k1 ,y obs,k0+k1 );

[0042] S4-4: Based on the tracked global path of the tracked vehicle obtained in step S1 and the motion boundary of the obstacle envelope circle obtained in step S4-2, calculate the position of the tracked vehicle centroid when the tracked vehicle envelope circle just crosses and is tangent to the motion boundary of the obstacle envelope circle while moving along the tracked global path of the tracked vehicle, and define as (x overtake ,y overtake );

[0043] S4-5: Based on the trajectory tracking motion control multi-constraint optimization problem constructed in step S3-6, construct a dynamic obstacle avoidance decision strategy trial and error mechanism: take the previous time step index k0-1 of the obstacle predicted position (x obs,k0 ,y obs,k0 ) corresponding to the predicted time step index k0 obtained in step S4-3, and based on the position (x overtake ,y overtake ) obtained in step S4-4, construct the predicted position constraint x k0-1 ≥ x overtake of the tracked vehicle at time step index k0-1; solve the trajectory tracking motion control multi-constraint optimization problem to obtain a control vector increment sequence, and use the control vector increment sequence to recursively solve the linearized time-discrete prediction model obtained in step S3-5 in the prediction time domain to obtain an output variable time sequence;

[0044] Verify whether the output variable time sequence satisfies the predicted position constraint of the tracked vehicle at time step index k0-1, if it satisfies, it means that the tracked vehicle can cross the motion boundary of the obstacle envelope circle obtained in step S4-2 before the obstacle enters the lateral motion deviation boundary of the tracked vehicle obtained in step S4-1, and realize the pre-emptive obstacle avoidance, at the same time, set the pre-emptive obstacle avoidance as the expected obstacle avoidance decision instruction of the tracked vehicle, and add the first group of vector elements in the control vector increment sequence to the tracked vehicle control variable at the previous time to obtain the expected obstacle avoidance motion control instruction at the current time; if it does not satisfy, construct a normal line tangent to the left side of the obstacle envelope circle corresponding to the position (x obs,k0 ,y obs,k0 ) to (x obs,k0+k1 ,y obs,k0+k1 ) in the expected global path in step S4-3, and project the intersection of the normal line and the expected global path to the x-axis of the tracked vehicle coordinate system to obtain the position (x yield,k0 ,y yield,k0 ) to (x yield,k0+k1 ,y yield,k0+k1 ), then construct the predicted position constraint x k ≤ x yield,k of the tracked vehicle from time step index k0 to k0+k1, where x yield,kThe multi-constraint optimization problem of trajectory tracking motion control is solved to obtain the control vector increment sequence. At the same time, the obstacle avoidance is set as the expected obstacle avoidance decision instruction of the tracked vehicle. The first group of vector elements in the control vector increment sequence is added to the control variables of the tracked vehicle at the previous moment to obtain the expected obstacle avoidance motion control instruction at the current moment, that is, the expected longitudinal acceleration a. x,ref and the desired yaw angular acceleration r d,ref .

[0045] The step S5 specifically includes:

[0046] S5-1: Based on the desired control instruction obtained in step S4-5, calculate the desired speed of the crawler vehicle motor:

[0047]

[0048] Among them, n 1,ref 、n 2,ref are the desired speeds of the left and right motors respectively; D is the track center distance; R is the equivalent rotation radius of the drive wheel;

[0049] S5-2: Use the PID controller to track the desired motor speed and obtain the motor drive torque control instruction, where K P,1 , K I,1 , K D,1 is the left motor controller parameter, K P,2 , K I,2 , K D,2 are the parameters of the motor controller on the right.

[0050] Compared with the prior art, the present invention has the following beneficial effects:

[0051] 1. A multi-constrained predictive optimization control method for tracked vehicle trajectory tracking and obstacle avoidance was constructed, incorporating multi-dimensional vehicle kinematic constraints and dynamic obstacle safety constraints to achieve efficient and safe trajectory tracking and collision avoidance capabilities.

[0052] 2. A trial-and-error mechanism for dynamic obstacle avoidance decision-making strategies is proposed, including dynamic overtaking obstacle avoidance decisions and dynamic yielding obstacle avoidance decisions, which improves the decision-making and control adaptability of tracked vehicles in dealing with dynamic and complex obstacle environments in the field.

[0053] In order to more clearly illustrate the functional characteristics and structural parameters of the present invention, further description is given below with reference to the accompanying drawings and specific embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Fig. 1 This is a schematic diagram of the unmanned tracked vehicle trajectory tracking and dynamic obstacle avoidance decision control process;

[0055] Fig. 2Schematic diagram of the principle of the dynamic obstacle avoidance decision strategy trial and error mechanism of the application - obstacle avoidance by giving way;

[0056] Fig. 3 Schematic diagram of the principle of the dynamic obstacle avoidance decision strategy trial and error mechanism of the application - obstacle avoidance by giving way. DETAILED DESCRIPTION

[0057] The technical solutions in the embodiments of the application will be clearly and completely described below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, rather than all the embodiments of the application. Based on the embodiments in the application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the application.

[0058] Please refer to Figs. 1-3 , Fig. 2 and Fig. 3 v x , v y is the longitudinal velocity and transverse velocity of the center of mass of the tracked vehicle in the vehicle coordinate system xoy; R trk , R obs is the envelope circle radius of the tracked vehicle and the obstacle, respectively;

[0059] (x ref,k , y ref,k ) is the reference trajectory point coordinate; (x obs,k , y obs,k ) is the obstacle prediction trajectory point coordinate;

[0060] (x overtake , y overtake ) is the position of the center of mass of the tracked vehicle when the envelope circle of the tracked vehicle just crosses the prediction motion boundary of the obstacle envelope circle and is tangent to it while moving along the expected global path of the tracked vehicle; (x yield,k , y yield,k ) is the position obtained by projecting the intersection of the normal line corresponding to the position (x obs,k0 , y obs,k0 ) to (x obs,k0+k1 , y obs,k0+k1 ) obtained in step S4-3 on the left side of the obstacle envelope circle and the expected global path onto the x-axis of the tracked vehicle coordinate system.

[0061] The application provides a technical solution: a dynamic obstacle avoidance decision control method for unmanned tracked vehicles in a field environment, comprising the following steps:

[0062] S1: receiving the output information of the global path planning and speed planning module in the unmanned driving system of the tracked vehicle, obtaining the expected global path and expected global speed of the tracked vehicle;

[0063] S2: collect the motion state of the tracked vehicle and the obstacle at the current time based on the vehicle-mounted sensor;

[0064] S3: based on the motion state of the tracked vehicle at the current time, and the tracked vehicle's expected global path and expected global speed obtained in step S1, use model predictive control to build a trajectory tracking and obstacle avoidance control multi-constraint optimization control problem;

[0065] S4: based on the trajectory tracking and obstacle avoidance control multi-constraint optimization control problem, and the motion state of the obstacle at the current time obtained in step S2, build a dynamic obstacle avoidance decision strategy trial-and-error mechanism to calculate the tracked vehicle's expected obstacle avoidance decision instruction and expected obstacle avoidance motion control instruction;

[0066] S5: build a tracked vehicle motion execution controller to track the expected obstacle avoidance motion control instruction to calculate the tracked vehicle motion execution control instruction;

[0067] S6: control the vehicle motion based on the tracked vehicle motion execution control instruction.

[0068] The step S2 specifically comprises:

[0069] S2-1: collect the motion state of the tracked vehicle and the obstacle at the current time, including: the global coordinates (X, Y) of the vehicle's center of mass in the earth coordinate system XOY; the longitudinal velocity v x and the lateral velocity v y of the vehicle's center of mass in the vehicle body coordinate system xoy; the yaw rate r of the vehicle at the center of mass; the vehicle heading angle θ; the longitudinal acceleration a x of the vehicle's center of mass in the vehicle body coordinate system; the yaw angular acceleration r d of the vehicle at the center of mass; the relative longitudinal distance Δx, the relative lateral distance Δy, the relative longitudinal velocity Δv x , and the relative lateral velocity Δv y of the vehicle and the obstacle in the vehicle body coordinate system.

[0070] The step S3 specifically comprises:

[0071] S3-1: based on the tracked vehicle's expected global path obtained in step S1, project the global coordinates of the tracked vehicle at the current time onto the expected global path to obtain the tracked vehicle global path projection point;

[0072] S3-2: take the tracked vehicle global path projection point as the origin, sample N1=20 times along the expected global path at a sampling time interval Δt=0.05s and the tracked vehicle's expected global speed obtained in step S1, and project the sampling points to the tracked vehicle body coordinate system to obtain the tracked vehicle reference trajectory points (x ref,k , y ref,k , θ ref,k);

[0073] S3-3: Construct the differential model of tracked vehicle motion control:

[0074]

[0075] where, are the first-order derivatives of v x , v y , r, κ = 0.55 is the side slip friction coefficient of tracked vehicle; g = 9.8 m / s 2 is the acceleration of gravity; σ = 0.02 is a small real number set; l = 2 m is the longitudinal length of the tracked vehicle footprint. On this basis, the kinematics equation of the tracked vehicle is constructed:

[0076]

[0077] where x and y are the longitudinal and lateral positions of the vehicle mass center in the body coordinate system, respectively; are the first-order derivatives of x, y, θ, respectively.

[0078] S3-4: Define the state variables of the tracked vehicle control state space model Output variables and control variables

[0079]

[0080] S3-5: Perform Taylor expansion at the reference operating point to construct a linearized time-discrete prediction model:

[0081]

[0082] where k is the prediction time step index; is the state variable at time k; are the state variable and output variable at time k+1, respectively; is the control variable at time k; I is the identity matrix; F(·) represents the differential model obtained by combining and augmenting the tracked vehicle motion control differential model and the kinematics equation constructed in step S3-3; is the partial differential symbol; is the reference operating point; Γ is the conversion matrix from the state variable to the output variable:

[0083]

[0084] S3-6: Construct the tracked vehicle trajectory tracking optimization control problem:

[0085]

[0086] where J is the total optimization objective; ΔU is the control variable increment sequence to be optimized, defined as follows:

[0087]

[0088] where Δu n = u n - u n-1 is the difference between the nth control variable u n and the (n-1)th control variable u n-1 ; N2 = 10 is the total number of steps in the control time domain. In addition, J track is the trajectory tracking accuracy optimization objective, J u and J Δu are the smoothness optimization objectives in the trajectory tracking process by suppressing the amplitude of the control variable and by reducing the rate of change of the control variable, respectively, which are defined as follows:

[0089]

[0090] where x is the output variable at time k; is the desired trajectory point vector of the tracked vehicle; Q, R u , R Δu are the weight matrices of the three optimization objectives.

[0091] Construct the constraints of the trajectory tracking and obstacle avoidance control optimization problem:

[0092]

[0093] where Γ x , Γ u , Γ Δu are matrices for converting the components of the state variable, the control variable, and the control variable rate of change vector that need to be constrained, respectively; is the lower limit and the upper limit of the components of the state variable that need to be constrained; is the lower limit and the upper limit of the components of the control variable that need to be constrained; is the lower limit and the upper limit of the components of the control variable rate of change vector that need to be constrained.

[0094] The step S4 specifically comprises:

[0095] S4-1: Based on the desired global path of the tracked vehicle obtained in step S1, construct the lateral motion deviation from the boundary of the tracked vehicle in the trajectory tracking process;

[0096] S4-2: Based on the motion state of the obstacle at the current time obtained in step S2, predict the motion of the obstacle to obtain a position prediction sequence point (xobs,k ,y obs,k ) and constructing the obstacle envelope circle prediction motion boundary;

[0097] S4-3: Based on the lateral motion deviation boundary of the tracked vehicle in the trajectory tracking process obtained in step S4-1 and the position prediction sequence points of the obstacle in the model prediction control prediction time domain, the obstacle envelope circle position prediction sequence points numbers within the lateral motion deviation boundary and intersecting with the boundary are calculated, and the predicted positions of the obstacle corresponding to the first and last numbers of the sequence points are defined as (x obs,k0 ,y obs,k0 ) and (x obs,k0+k1 ,y obs,k0+k1 );

[0098] S4-4: Based on the tracked vehicle expected global path obtained in step S1 and the obstacle envelope circle prediction motion boundary obtained in step S4-2, the tracked vehicle centroid position when the tracked vehicle envelope circle just crosses the obstacle envelope circle prediction motion boundary and is tangent to it along the tracked vehicle expected global path is calculated, and is defined as (x overtake ,y overtake );

[0099] S4-5: Based on the trajectory tracking motion control multi-constraint optimization problem constructed in step S3-6, a dynamic obstacle avoidance decision strategy trial and error mechanism is constructed: the previous time step index k0-1 corresponding to the obstacle prediction position (x obs,k0 ,y obs,k0 ) obtained in step S4-3 is taken out, and based on the position (x overtake ,y overtake ) obtained in step S4-4, the predicted position constraint x k0-1 ≥x overtake of the tracked vehicle at time step index k0-1 is constructed; on this basis, the trajectory tracking motion control multi-constraint optimization problem is solved to obtain the control vector increment sequence, and the control vector increment sequence is used to recursively predict the linearized time discrete prediction model obtained in step S3-5 in the prediction time domain to obtain the output variable time sequence; it is verified whether the output variable time sequence satisfies the above predicted position constraint of the tracked vehicle at time step index k0-1, if it satisfies, it means that the tracked vehicle can cross the obstacle envelope circle prediction motion boundary obtained in step S4-2 before the obstacle enters the tracked vehicle lateral motion deviation boundary obtained in step S4-1, and realize the rush avoidance, at the same time, set the rush avoidance as the tracked vehicle expected obstacle avoidance decision instruction, and add the first group of vector elements in the control vector increment sequence to the tracked vehicle control variable at the last time to obtain the expected obstacle avoidance motion control instruction at the current time; if it does not satisfy, the expected global path is constructed to obtain the position (x obs,k0 ,yobs,k0 ) to (x obs,k0+k1 ,y obs,k0+k1 ) corresponds to the left tangent of the normal of the obstacle envelope circle, and the intersection of the normal and the desired global path is projected to the x-axis of the tracked vehicle coordinate system to obtain the position (x yield,k0 ,y yield,k0 ) to (x yield,k0+k1 ,y yield,k0+k1 ), then the predicted position constraint x k ≤x yield,k of the tracked vehicle from the time step index k0 to k0+k1 is constructed, where x yield,k is the position boundary at time k, on this basis, the trajectory tracking motion control multi-constraint optimization problem is solved to obtain the control vector increment sequence, at the same time, the yielding obstacle avoidance is set as the expected obstacle avoidance decision instruction of the tracked vehicle, and the first group of vector elements in the control vector increment sequence is added to the tracked vehicle control variable at the last time to obtain the expected obstacle avoidance motion control instruction at the current time, i.e. the expected longitudinal acceleration a x,ref and the expected yaw angular acceleration r d,ref .

[0100] The step S5 specifically comprises:

[0101] S5-1: based on the expected control instruction obtained in step S4-5, the expected speed of the tracked vehicle motor is calculated:

[0102]

[0103] wherein n 1,ref and n 2,ref are the expected speeds of the left and right motors respectively; D = 1.7 m is the tracked center distance; R = 0.2 m is the equivalent rotation radius of the driving wheel;

[0104] S5-2: the expected speed of the motor is tracked by using a PID controller to obtain the motor driving torque control instruction, wherein K P = 5, K I = 0.5, and K D = 0.1 are the motor controller parameters.

[0105] Although embodiments of the present application have been shown and described, it is to be understood that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the present application, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A dynamic obstacle avoidance decision control method for an unmanned tracked vehicle in a field environment, characterized in that the method include: S1: Receive the output information of the global path planning and speed planning module in the unmanned driving system of the tracked vehicle, and obtain the expected global path and expected global speed of the tracked vehicle; S2: Obtain the motion status of the tracked vehicle and the obstacle at the current moment based on the onboard sensors; S3: Based on the current motion state of the tracked vehicle and the desired global path and desired global speed of the tracked vehicle obtained in step S1, a multi-constrained optimization control problem of trajectory tracking and obstacle avoidance control is constructed using model predictive control; S4: Based on the multi-constraint optimization control problem of trajectory tracking and obstacle avoidance control, and the current motion state of the obstacle obtained in step S2, a dynamic obstacle avoidance decision strategy trial-and-error mechanism is constructed to obtain the desired obstacle avoidance decision instructions and desired obstacle avoidance motion control instructions for the tracked vehicle; S5: Constructing a crawler vehicle motion execution controller to track the desired obstacle avoidance motion control instruction and obtain the crawler vehicle motion execution control instruction; S6: Execute control instructions based on the movement of the tracked vehicle to control the movement of the vehicle.

2. The method according to claim 1, characterized in that The current motion status of the tracked vehicle and obstacles is obtained based on the onboard sensors, including: S2-1: Collect the motion state of the tracked vehicle and the obstacle at the current moment. The motion state includes: the global coordinates (X, Y) of the vehicle's center of mass in the geodetic coordinate system XOY; the longitudinal velocity v of the vehicle's center of mass in the body coordinate system xoy x and the lateral velocity v y ; The yaw rate r of the vehicle at the center of mass; The vehicle heading angle θ; The longitudinal acceleration a of the vehicle center of mass in the body coordinate system x ; The vehicle's yaw angular acceleration at the center of mass r d ; Relative longitudinal distance Δx, relative lateral distance Δy, and relative longitudinal speed Δv between the vehicle and the obstacle in the vehicle body coordinate system x , relative lateral velocity Δv y .

3. The method according to claim 2, characterized in that S3: Based on the current motion state of the tracked vehicle and the desired global path and desired global speed of the tracked vehicle obtained in step S1, a multi-constrained optimization control problem for trajectory tracking and obstacle avoidance control is constructed using model predictive control, including: S3-1: Based on the expected global path of the tracked vehicle obtained in step S1, the global coordinates of the tracked vehicle at the current moment are projected onto the expected global path to obtain a projection point of the tracked vehicle global path; S3-2: With the projection point of the tracked vehicle global path as the origin, perform N1 sampling along the expected global path according to the sampling time interval Δt and the expected global speed of the tracked vehicle obtained in step S1, and project the sampling points to the tracked vehicle body coordinate system to obtain the tracked vehicle reference trajectory point (x ref,k ,y ref,k ,θ ref,k ), where the three coordinates are the longitudinal position, lateral position, and heading angle of the kth desired trajectory point; S3-3: Constructing a differential model for tracked vehicle motion control: in, v x 、v y , the first-order derivative of r; κ is the sideslip friction coefficient of the tracked vehicle; g is the acceleration due to gravity; tanh is the hyperbolic tangent function; σ is a set small real number; l is the longitudinal length of the tracked vehicle contact patch; Construct the kinematic equations for the tracked vehicle: Among them, x and y are the longitudinal and lateral positions of the vehicle's center of mass in the body coordinate system respectively; are the first-order derivatives of x, y, and θ respectively; S3-4: Define the state variables of the tracked vehicle control state space model Output variables and control variables S3-5: Perform Taylor expansion at the reference operating point to construct a linearized time-discrete prediction model: Among them, k is the index of the prediction time step; is the state variable at time k; are the state variables and output variables at time k+1 respectively; is the control variable at time k; I is the unit matrix; F(·) represents the differential model obtained by combining and augmenting the crawler vehicle motion control differential model and the kinematic equation constructed in step S3-3; is the partial differential symbol; is the reference operating point; Γ is the conversion matrix from state variables to output variables; S3-6: Construct track tracking optimization control problem for tracked vehicles: Where J is the overall optimization objective; ΔU is the incremental sequence of the control variables to be optimized, which is defined as follows: Where Δu n =u n -u n-1 is the nth control variable u n and the n-1th control variable u n-1 The difference between the two; N2 is the total number of steps in the control time domain; In addition, J track To optimize the trajectory tracking accuracy, J u and J Δu They are the optimization objectives of stability by suppressing the amplitude of the control variable during trajectory tracking and the optimization objectives of stability by reducing the rate of change of the control variable. The three are defined as follows: in, is the output variable at time k; is the expected trajectory point vector of the tracked vehicle; Q, R u 、R Δu are the weight matrices corresponding to the three optimization objectives respectively; Construct the trajectory tracking and obstacle avoidance control optimization problem constraints: Among them, Γ x , Γ u , Γ Δu are matrices for transforming the state variables, control variables, and components of the control variable change rate vector that need to be constrained; are the lower and upper limits of the components in the state variables that need to be constrained; is the lower and upper limits of the components that need to be constrained in the control variables; are the lower and upper limits of the components in the control variable change rate vector that need to be constrained.

4. The method according to claim 3, characterized in that S4: Based on the multi-constraint optimization control problem of trajectory tracking and obstacle avoidance control, and the current motion state of the obstacle obtained in step S2, a dynamic obstacle avoidance decision strategy trial-and-error mechanism is constructed to obtain the desired obstacle avoidance decision instructions and desired obstacle avoidance motion control instructions for the tracked vehicle; including: S4-1: Based on the expected global path of the tracked vehicle obtained in step S1, a lateral motion deviation boundary of the tracked vehicle during the trajectory tracking process is constructed; S4-2: Based on the current motion state of the obstacle obtained in step S2, the obstacle motion is predicted to obtain the position prediction sequence points (x obs,k ,y obs,k ), and construct the obstacle envelope to predict the motion boundary; S4-3: Based on the lateral motion deviation boundary of the tracked vehicle during the trajectory tracking process obtained in step S4-1 and the position prediction sequence points of the obstacle in the model predictive control prediction time domain, the obstacle envelope position prediction sequence point numbers within the lateral motion deviation boundary and intersecting with the boundary are calculated and screened, and the obstacle prediction position corresponding to the first and last sequence point numbers is defined as (x obs,k0 ,y obs,k0 )and (x obs,k0+k1 ,and obs,k0+k1 ); S4-4: Based on the expected global path of the tracked vehicle obtained in step S1 and the predicted motion boundary of the obstacle envelope obtained in step S4-2, calculate the center of mass position of the tracked vehicle when the tracked vehicle envelope just crosses the predicted motion boundary of the obstacle envelope and is tangent to it when the tracked vehicle moves along the expected global path of the tracked vehicle, and define it as (x overtake ,y overtake ); S4-5: Based on the multi-constraint optimization problem of trajectory tracking motion control constructed in step S3-6, a dynamic obstacle avoidance decision-making strategy trial-and-error mechanism is constructed: the obstacle prediction position (x obs,k0 ,y obs,k0 ) The previous time step index k0-1 corresponding to the prediction time step index k0 is taken out, and based on the position (x overtake ,y overtake ), construct the predicted position constraint x of the tracked vehicle at time step index k0-1 k0-1 ≥x overtake Solve the multi-constraint optimization problem of trajectory tracking motion control to obtain a control vector increment sequence, and use the control vector increment sequence to recursively predict the linearized time discrete prediction model obtained in step S3-5 in the prediction time domain to obtain an output variable time series; Verify whether the output variable time series satisfies the predicted position constraint of the tracked vehicle at the time step index k0-1. If so, it means that the tracked vehicle can cross the predicted motion boundary of the obstacle envelope obtained in step S4-2 before the obstacle enters the tracked vehicle lateral motion deviation boundary obtained in step S4-1, thereby achieving rushing and avoiding obstacles. At the same time, set rushing and avoiding obstacles as the tracked vehicle's expected obstacle avoidance decision instruction, and add the first group of vector elements in the control vector increment sequence to the tracked vehicle control variables at the previous moment to obtain the expected obstacle avoidance motion control instruction at the current moment; if not, construct the position (x) obtained in step S4-3 on the expected global path. obs,k0 ,y obs,k0 ) to (x obs,k0+k1 ,y obs,k0+k1 ) is tangent to the left side of the obstacle envelope corresponding to the normal line, and the intersection of the normal line and the expected global path is projected onto the x-axis of the tracked vehicle coordinate system to obtain the position (x yield,k0 ,y yield,k0 ) to (x yield,k0+k1 ,y yield,k0+k1 ), then construct the predicted position constraint x of the tracked vehicle from time step index k0 to k0+k1 k ≤x yield,k , where x yield,k The multi-constraint optimization problem of trajectory tracking motion control is solved to obtain the control vector increment sequence. At the same time, the obstacle avoidance is set as the expected obstacle avoidance decision instruction of the tracked vehicle. The first group of vector elements in the control vector increment sequence is added to the control variables of the tracked vehicle at the previous moment to obtain the expected obstacle avoidance motion control instruction at the current moment, that is, the expected longitudinal acceleration a. x,ref and the desired yaw angular acceleration r d,ref .

5. The method according to claim 4, characterized in that S5: Constructing a crawler vehicle motion execution controller to track the desired obstacle avoidance motion control instructions to obtain the crawler vehicle motion execution control instructions; including: S5-1: Based on the desired control instruction obtained in step S4-5, calculate the desired speed of the crawler vehicle motor: Among them, n 1,ref 、n 2,ref are the desired speeds of the left and right motors respectively; D is the track center distance; R is the equivalent rotation radius of the drive wheel; S5-2: Use the PID controller to track the desired motor speed and obtain the motor drive torque control instruction, where K P,1 , K I,1 , K D,1 is the left motor controller parameter, K P,2 , K I,2 , K D,2 are the parameters of the motor controller on the right.

Images