A motion control method for unmanned tracked vehicles on multi-directional slope sections

By using predictive models of lidar and geodetic coordinate systems on unmanned tracked vehicles, a dynamic model of multi-directional ramp sections is constructed, and the driving force and braking force is optimized, the vehicle trajectory tracking problem in complex terrain environments is solved and the vehicle is improved stability and safety.

CN118651244BActive Publication Date: 2024-11-05JIANGSU IND INNOVATION CENT OF INTELLIGENT EQUIP CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411150519.8
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2024-08-21
Publication Date
2024-11-05
Estimated Expiration
2044-08-21

AI Technical Summary

Technical Problem

The existing unmanned tracked vehicle motion control technology has limitations in complex and changing slope terrain environments, and it is particularly difficult to consider longitudinal slopes and side slopes at the same time, resulting in poor tracking of vehicles in complex terrain environments, and forced stops, Risks such as speeding and steering not falling off the cliff in time.

Method used

Using prediction models based on lidar and geodetic coordinate systems, the prediction logic of the vehicle's pre-arrival position is constructed through point cloud information, equivalent slope angle and direction angle are calculated, coupled dynamics model is established, and the vehicle's driving force or system is optimized in real time. Power, realize effective control of multi-directional ramp sections.

Benefits of technology

Effectively predict and optimize the movement of the vehicle in complex slope environments, improve the stability and trajectory tracking accuracy of the vehicle, avoid forced stops, slope slips and rush off the road, and improve the vehicle's execution efficiency and safety.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118651244B_ABST
    Figure CN118651244B_ABST
Patent Text Reader

Abstract

The present invention discloses a motion control method for an unmanned tracked vehicle on a multi-directional slope section, the method comprising the following steps: establishing a geodetic coordinate system; obtaining a vehicle reference trajectory, vehicle position information and original point cloud information of a laser radar; converting the original point cloud information into complete point cloud information under the geodetic coordinate system based on the geodetic coordinate system and the vehicle position information; constructing a prediction model to predict the vehicle's pre-arrival position according to the vehicle's current position and state; setting constraints and an objective function; calling a model prediction controller to calculate an optimal control sequence based on the prediction model, constraints and the objective function; calling an execution unit of the vehicle to control the driving force or braking force of the vehicle according to the optimal control sequence; the present invention can effectively predict the side slope and longitudinal slope terrain in the front driving path of the unmanned tracked vehicle, optimize the driving force or braking force of the vehicle track through prediction, and can more effectively control the motion of the unmanned tracked vehicle.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving control technology. Specifically, the present invention is applied to the field of unmanned tracked vehicle control technology, and in particular to a motion control method for an unmanned tracked vehicle on a multi-directional slope section. Background Art

[0002] In recent years, unmanned tracked vehicles have been increasingly used in military, exploration, agriculture, and disaster relief. However, due to their large mass, large moment of inertia, under-actuated characteristics, and large off-road terrain changes, the automatic driving control of unmanned tracked vehicles is difficult, especially when facing complex and changeable slope terrain conditions, which puts higher requirements on the motion control system of tracked vehicles, as follows:

[0003] First, the vehicle speed stability will be significantly affected by the longitudinal slope. The delay / slope limit / amplitude limit of the control response may cause the vehicle to be unable to adjust the driving force or braking force in real time on the longitudinal slope, causing the vehicle to drop sharply or even be forced to stop when going uphill, or lose control of the speed and run away when going downhill.

[0004] Secondly, the vehicle's steering stability will also be significantly affected by the side slope. Unpredicted slope changes will make it difficult for the vehicle to adjust the driving force in real time for accurate steering, and thus it will not be able to accurately achieve autonomous driving path tracking. If the control is not appropriate, it is easy to run out of the predetermined trajectory of autonomous driving, causing risks such as falling off a cliff and rollover.

[0005] In view of the above situation, the existing scheme proposes a variety of motion control methods for unmanned tracked vehicles, as follows:

[0006] (I) A control scheme with closed-loop feedback for the current ramp state: CN117719464A discloses a hybrid tracked vehicle electromechanical combined emergency braking stability control method and vehicle taking into account the road slope, which adopts feedforward and PID control to improve the braking efficiency and safety of the tracked vehicle on the longitudinal ramp; by integrating the slope sensing system, it can adjust the braking strategy in real time to ensure that it matches the vehicle's speed, slope and tail angle; it uses the motor torque for feedforward and PID control to accurately control the braking force and optimize the synergy of mechanical and motor braking; but its feedforward + PID control focuses on making adjustments based on the current error feedback, which means that it is mainly responsive and adjusts to the current ramp state, but lacks foresight and foresight for future states.

[0007] (ii) A scheme for predictive control of future reference trajectories: CN114510063A discloses an unmanned tracked vehicle and a trajectory tracking control method and system thereof; it first generates an objective function of a trajectory tracking controller based on a kinematic model of the unmanned tracked vehicle, and generates a constraint function of the trajectory tracking controller based on this function and a dynamic model of the unmanned tracked vehicle, thereby forming an MPC model; then, after generating a reference trajectory according to the posture characteristics of the unmanned tracked vehicle, the curvature characteristics of the road and the real-time working condition status information, the reference trajectory and the vehicle trajectory tracking response status information are input into the MPC model to obtain a parameter combination; then, the parameter combination is input into the MLP neural network to obtain control parameters; finally, the tracking control of the unmanned tracked vehicle trajectory is completed according to the control parameters, thereby effectively improving the working condition adaptability of the unmanned tracked vehicle trajectory tracking and achieving a better balance among trajectory tracking accuracy, vehicle driving stability and calculation time cost; however, this patent mainly considers the curvature characteristics of the road, and does not focus on the slope characteristics of the road ahead.

[0008] (III) Predictive control scheme based on vehicle dynamics model: CN114355882A discloses a method for designing a track tracking controller for a tracked unmanned vehicle based on a dynamics model; first, a state space equation is established according to the tracked vehicle dynamics model; second, a standard quadratic transformation is performed according to the set objective function; in the next control cycle, the system calculates a new control sequence through the optimization process of the objective function, and repeatedly performs rolling optimization to achieve track tracking control, so as to achieve high-precision and stable tracking capabilities for the vehicle under various speed conditions; however, the predictive model of this patent does not take into account the changes in dynamic characteristics caused by slope conditions.

[0009] (IV) Perform a certain degree of predictive control for future terrain: CN117565870B discloses a method for predictive control of ultra-low speeds on sloped sections of off-road unmanned vehicles; this method first uses a laser radar to collect road point cloud data in front of the vehicle, and combines the data obtained by real-time correction of the inertial navigation system to calculate the longitudinal profile data of the vehicle's direction of travel; then, these longitudinal profile data are processed to calculate the slope angle function of the road ahead, that is, the slope change that the vehicle is about to face; through the model predictive control strategy, combined with the current state of the vehicle and the predicted slope angle information, the vehicle's driving force and braking force are relatively optimized; however, this method does not consider the impact of lateral slopes such as side slopes on vehicle stability, and does not discuss steering control.

[0010] In summary, existing solutions have made certain progress in the motion control of unmanned tracked vehicles, but their application in complex terrain environments still shows obvious limitations; for example, existing tracked vehicle motion control technology focuses on the vehicle's driving on the longitudinal slope, while there is less research on the motion control of unmanned tracked vehicles that takes into account both longitudinal and lateral slopes, and there is a clear lack of research on predicting the ramp ahead to control the path of unmanned tracked vehicles; this results in the lack of effective prediction of the ramp ahead in the trajectory tracking of tracked vehicles in off-road environments, and the actual control effect is poor, with the risk of causing the vehicle to be forced to stop, run away, or fall off a cliff due to untimely steering. Summary of the invention

[0011] The object of the present invention is to provide a motion control method for an unmanned tracked vehicle on a multi-directional slope section, thereby solving all or one of the above-mentioned problems existing in the prior art.

[0012] In order to solve the above technical problems, the specific technical solutions of the present invention are as follows:

[0013] In one aspect, the present invention provides a method for controlling the motion of an unmanned tracked vehicle on a multi-directional slope section, comprising the following steps:

[0014] Initialization steps:

[0015] Establishing a geodetic coordinate system; acquiring a vehicle reference trajectory, vehicle position information, and original point cloud information of a laser radar; and converting the original point cloud information into complete point cloud information in the geodetic coordinate system based on the geodetic coordinate system and the vehicle position information;

[0016] Vehicle control steps:

[0017] Construct a prediction model, wherein the prediction model is used to predict the vehicle's expected arrival position based on the vehicle's current position and status; call the model prediction controller to calculate the optimal control sequence based on the prediction model, the constraints and the objective function; and call the vehicle's execution unit to control the vehicle's driving force or braking force based on the optimal control sequence.

[0018] In one embodiment, the prediction logic of the prediction model for the estimated arrival position of the vehicle includes:

[0019] Confirming the current position of the vehicle in the complete point cloud information;

[0020] Calculating a plane normal vector at the current position of the vehicle;

[0021] Calculate the equivalent slope angle at the current position of the vehicle and the direction angle of the vehicle traveling on the slope according to the plane normal vector;

[0022] Determine the motion state equation of the vehicle according to the current position of the vehicle, the equivalent slope angle and the direction angle;

[0023] The expected arrival position of the vehicle is predicted according to the motion state equation.

[0024] In one implementation manner, calculating the plane normal vector at the current position of the vehicle includes:

[0025] Define the current position of the vehicle as ( , , );

[0026] In the complete point cloud information, a point cloud subset is selected to represent the vehicle attachment condition at the current position of the vehicle. ;

[0027] Calculate the point cloud subset The fitting plane equation and covariance matrix of ;

[0028] Determining the optimal condition when fitting the plane according to the covariance matrix;

[0029] The normal vector of the fitting plane is determined according to the optimal condition as the plane normal vector at the current position of the vehicle.

[0030] In one embodiment, the calculating the equivalent slope angle at the current position of the vehicle and the direction angle of the vehicle traveling on the ramp according to the plane normal vector includes:

[0031] Substituting the dot product between the normal vector of the horizontal plane and the plane normal vector at the current position of the vehicle into the cosine value formula of the angle between the horizontal plane and the fitting plane, and calculating the equivalent slope angle by arc cosine;

[0032] The heading angle of the vehicle is subtracted from the angle between the projection of the plane normal vector at the current position of the vehicle on the horizontal plane and the X-axis, and then subtracted from π to obtain the direction angle.

[0033] In one embodiment, the motion state equation includes:

[0034] ;

[0035] In the motion state equation, G is the vehicle weight, m ​​is the vehicle mass, L is the track ground length, B is the track center distance, θ is the slope angle, φ is the direction angle, and h is the height of the center of gravity from the ground. is the steering resistance coefficient when turning on a slope, and J is the moment of inertia;

[0036] In the equation of motion, and are the driving resistance of the ground acting on the vehicle tracks, M is the steering resistance torque of the ground acting on the vehicle tracks, and M are values ​​that vary with θ and φ;

[0037] In the equation of motion, and They are respectively the driving force or braking force transmitted to the tracks from the driving wheels on both sides of the vehicle;

[0038] In the motion state equation, v is the vehicle speed, which points to the front of the vehicle; ω is the steering angular velocity, which is the rate of change of the vehicle heading angle α.

[0039] In one embodiment, predicting the expected arrival position of the vehicle according to the motion state equation includes:

[0040] The motion state equation is discretized to obtain a discrete equation:

[0041] Calculating a discrete estimated relationship between the position and heading angle of the vehicle according to the discrete equation;

[0042] The vehicle speed, vehicle steering angular velocity and vehicle position at the next moment under the set driving force or braking force are calculated based on the discrete estimated relationship, the vehicle speed, vehicle steering angular velocity and vehicle position at the previous moment.

[0043] In one embodiment, the constraint condition includes:

[0044] ;

[0045] The constraint condition is a constraint condition on driving force or braking force.

[0046] In one embodiment, the objective function includes:

[0047] ;

[0048] In the objective function, To adjust the parameters affecting the weights of the four sub-goals;

[0049] In the objective function, N is the number of cycles of predictive control;

[0050] In the objective function, The algorithm is to traverse the vehicle reference trajectory and find the waypoints W i , find the Smallest W i for The corresponding ; is the current vehicle position.

[0051] In one embodiment, the optimal control sequence includes:

[0052] = .

[0053] In one embodiment, calling the execution unit of the vehicle to control the driving force or braking force of the vehicle according to the optimal control sequence includes:

[0054] The first item in the obtained optimal control sequence As the control value at the current moment, it is sent to the drive unit or brake unit to execute the driving force or braking force.

[0055] The beneficial effects of the technical solution of the present invention are:

[0056] 1. The motion control method of an unmanned tracked vehicle on a multi-directional slope section described in the present invention can effectively predict the side slope and longitudinal slope terrain in the driving path ahead of the unmanned tracked vehicle, optimize the driving force or braking force of the vehicle track through prediction, and can more effectively control the motion of the unmanned tracked vehicle.

[0057] 2. The motion control method of the unmanned tracked vehicle on a multi-directional slope section described in the present invention can, based on advanced sensors, control algorithms and vehicle dynamics prediction control models, predict the slope angle of the terrain that the vehicle may encounter in the future and calculate the optimal control sequence when the tracked vehicle is on a slope section with complex undulations all around. Finally, the driving force or braking force of the tracks on both sides of the vehicle is optimized according to the optimal control sequence, so as to achieve stable control of the vehicle speed and steering angular velocity, achieve stable tracking of the reference trajectory, improve the stability of the vehicle, avoid forced parking, sliding down the slope, and running off the road, etc., improve the vehicle's execution efficiency and execution safety for the task, make up for the defects of the existing technology, and have high application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0059] Figure 1 is a flow chart of a motion control method for an unmanned tracked vehicle on a multi-directional slope section according to Embodiment 1 of the present invention;

[0060] Figure 2 is a schematic diagram of the earth coordinate system in the motion control method of the unmanned tracked vehicle on the multi-directional slope section described in Example 1 of the present invention;

[0061] Figure 3 It is a schematic diagram of the rotation of coordinate axes in the motion control method of an unmanned tracked vehicle on a multi-directional slope section described in Example 1 of the present invention;

[0062] Figure 4 is a schematic diagram of an equivalent slope angle in the motion control method of an unmanned tracked vehicle on a multi-directional slope section according to Embodiment 1 of the present invention;

[0063] Figure 5 It is a schematic diagram of the force analysis of the tracked vehicle on the slope plane in the motion control method of the unmanned tracked vehicle on the multi-directional slope section described in Example 1 of the present invention;

[0064] Figure 6 is a schematic diagram of the direction angle in the motion control method of an unmanned tracked vehicle on a multi-directional slope section according to Embodiment 1 of the present invention;

[0065] Figure 7 It is a schematic diagram of the system architecture of the motion control system of the unmanned tracked vehicle on the multi-directional slope section described in Example 2 of the present invention. DETAILED DESCRIPTION

[0066] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby making a clearer and more definite definition of the protection scope of the present invention.

[0067] In the description of the present invention, it should be noted that the embodiments described in the present invention are only part of the embodiments of the present invention, rather than all of the embodiments; based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present invention.

[0068] The terms "first", "second", etc. in the specification and claims of this article and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of this article described here can be implemented in an order other than those illustrated or described here. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, device, product or equipment that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or equipment.

[0069] Embodiment 1:

[0070] This embodiment provides a motion control method for an unmanned tracked vehicle on a multi-directional slope section. Figure 1 to Figure 6 As shown, the following steps are included:

[0071] The core functions of this method are: predicting the terrain on the reference track that the vehicle is about to travel based on the laser radar, calculating the slope angle at any position based on the ramp plane fitting and the ramp normal vector, and judging the vehicle orientation at a given position based on the vehicle heading angle. Finally, a coupled dynamic prediction model for the longitudinal slope and side slope ahead is established, and real-time predictive control is performed based on the model to achieve real-time adjustment of the driving force / braking force of the active wheels on both sides of the crawler vehicle, thereby improving the vehicle's motion stability and tracking accuracy when traveling along the reference track in a complex slope environment. The specific steps are as follows:

[0072] S100, establish coordinate system:

[0073] In this step, the geodetic coordinate system O- ;

[0074] Specifically, Figure 2 As shown, the geodetic coordinate system O- Middle: X is toward the east; Y is toward the north; Z is vertically upward; the origin O is a fixed point on the earth and cannot be changed after selection; it should be noted that the geodetic coordinate system O- Keeping consistent with the coordinate system of the navigation system, thereby ensuring that the reference trajectory provided by the method and the positioning system is measured in the same coordinate system;

[0075] Specifically, regarding the geodetic coordinate system O- The schematic diagram of the rotation of the three coordinate systems is as follows Figure 3 As shown, the corresponding three rotation matrices are as follows:

[0076] ;

[0077] ;

[0078] .

[0079] S200, obtaining vehicle reference trajectory:

[0080] In this step, the vehicle reference trajectory refers to the reference trajectory given by the received navigation system, which is used as the reference trajectory Waypoints for the vehicle to travel;

[0081] Specifically, each waypoint W in Waypoints i The coordinate value of (W i .x, W i .y, Wi .α).

[0082] S300, obtaining vehicle location information:

[0083] In this step, the vehicle position information Veh (x, y, z, α, β, γ) of the positioning system is received;

[0084] Specifically, Veh(x, y, z, α, β, γ) refers to the coordinate value of the vehicle in the geodetic coordinate system fed back by the positioning system; where α is the angle of rotation around the Z axis, β is the angle of rotation around the Y axis, and γ is the angle of rotation around the X axis.

[0085] S400, obtaining point cloud information at the current moment;

[0086] In this step, the original point cloud information of the laser radar is received; based on the geodetic coordinate system and vehicle position information And the coordinate conversion algorithm calculates the complete point cloud information P at the current moment based on the geodetic coordinate system;

[0087] It should be noted that, since the laser radar data is measured based on the vehicle's moving coordinate system, its point cloud coordinate system takes the center of mass of the vehicle as the origin, the front direction of the vehicle is the x direction, the left direction of the vehicle is the y direction, and the roof direction is the z direction; however, in this method, the slope, trajectory tracking and tracked vehicle dynamics are all measured or studied based on the geodetic coordinate system, so it is necessary to perform coordinate conversion on the original point cloud information of the laser radar, so this step is essentially a coordinate conversion step for the original point cloud data;

[0088] Specifically, the above coordinate conversion algorithm is as follows:

[0089] = · · · + ;

[0090] in:

[0091] Porigin(x,y,z) is the position value of any point p in the lidar point cloud at any time;

[0092] p(x,y,z) is the position value of point p transformed into the geodetic coordinate system;

[0093] Through the above algorithm, any lidar data point can be projected into the geodetic coordinate system for coordinate transformation; finally, by traversing the lidar point cloud, the complete point cloud information P in the geodetic coordinate system at the current moment can be obtained.

[0094] S500, vehicle arrival position prediction:

[0095] In this step, a prediction model is established and the prediction model is called according to the given as well as , further extrapolated ; Among them, the current time is time 0, and the time interval is , k represents the value of the corresponding variable at time k, and k+1 represents the value of the corresponding variable at time k+1;

[0096] Specifically, in the predictive control process, the prediction model is continuously called to continuously predict and update the position that the tracked vehicle will reach under the given control, which involves the calculation of the plane normal vector at the given position, the calculation of the equivalent slope angle θ at the given position, the calculation of the direction angle φ of the vehicle traveling on the ramp, and the determination of the motion state equation;

[0097] Specifically, the prediction calculation logic of the prediction model is as follows:

[0098] S501, according to the given geodetic coordinate system Location( , , ), and the aforementioned laser radar point cloud P determines the plane normal vector at that location ;

[0099] S5011, for location ( , , ), in the complete lidar point cloud P, select the point cloud subset that can represent the attachment of the tracked vehicle here ; Where L is the track ground length, the points obtained by screening Together they form a new point cloud subset ;

[0100] S5012, for , calculate its fitting plane equation and covariance matrix A;

[0101] Among them, about the fitting plane equation:

[0102] Since the fitting plane should pass through the point ( , , ), so the fitting plane equation is set to , set the constraints to + = 1 (if the constraint condition can be satisfied by adjusting the d value in normalized form), then the vector , which is the unit normal vector;

[0103] Among them, about the covariance matrix A:

[0104] against Calculate the average coordinates of all its points ;for Any point in ; Based on the above fitting plane equation, the distance from any point to the plane is ;when When the minimum value is taken, the best fitting plane is obtained; finally, based on the Lagrangian method, the constructor ; Calculate the partial derivatives of parameters a, b, c, and d respectively and set the partial derivatives to 0, and the covariance matrix A is obtained as follows:

[0105] ;

[0106] and ;

[0107] S5013, calculating the normal vector of the fitting plane based on the covariance matrix A;

[0108] Among them, there is a characteristic equation AX= X, is the eigenvector of A, and has more than one eigenvalue; + =1, so the inner product of the eigenvectors (X,X)=1, (AX,X)=( X,X), so =(AX,X)= ; According to the optimal condition for fitting the plane: Take the minimum, so the minimum value is selected in the calculated eigenvalues, and its corresponding eigenvector is used as the normal vector of the fitting plane, which is .

[0109] It should be noted that when calculating the normal vector, the z-axis component of the normal vector is specified ;exist( , , ), since the given reference trajectory is a drivable route, the LiDAR point cloud at this location can represent the ground point cloud; by fitting the LiDAR point cloud at a given location to a plane, when the unmanned vehicle approaches this location, it should attach to this plane, and calculate the normal vector of the fitted plane, which can provide data support for subsequent slope measurement.

[0110] S502, according to the given geodetic coordinate system Location( , , ), and the normal vector of the laser point cloud fitting plane at that location (that is, when the vehicle drives to this location) ,calculate The equivalent slope angle θ at and the direction angle φ of the vehicle traveling on the slope;

[0111] S5021, calculating an equivalent slope angle θ according to the normal vector of the ramp;

[0112] Among them, the normal vector of the known horizontal plane is , at this time, the normal vector of the slope in front of the crawler vehicle is known to be , so the cosine of the angle between the two planes is ,in is the dot product of two vectors, and the equivalent slope angle θ can be calculated by arc cosine;

[0113] It should be noted that if Figure 4 As shown in the figure, the equivalent slope angle is the angle between the slope and the horizontal plane, which affects the force of the tracked vehicle on the slope, especially the rolling resistance; and the intersection line of the ramp plane and the horizontal plane is not necessarily parallel to the Y axis or the X axis;

[0114] S5022, such as Figure 6 As shown, the formula Calculate the direction angle φ;

[0115] Among them, δ is the angle between the projection of the ramp normal vector on the horizontal plane and the X-axis;

[0116] in, (Depending on the specific situation is 0);

[0117] Where α is the given heading angle of the tracked vehicle;

[0118] Among them, the projection of the ramp normal vector on the horizontal plane is ;

[0119] The direction angle refers to the angle between the center line of the tracked vehicle and the center line of the ramp, such as Figure 4 As shown, it also affects the forces on tracked vehicles, especially the steering resistance torque;

[0120] S503, according to the given geodetic coordinate system Location( , , ), and the vehicle is located at The equivalent slope angle θ and direction angle φ at the position are used to determine its motion state equation; the update strategy of the tracked vehicle position is determined according to the motion state equation when the driving force / braking force of the driving wheels on both sides is given, and then the ;

[0121] S5031. The motion state equation of the tracked vehicle when it moves on a slope with an equivalent slope angle of θ and a direction angle of φ is as follows:

[0122] ;

[0123] Among them, G is the vehicle weight, m ​​is the vehicle mass, L is the track ground length, B is the track center distance, θ is the slope angle, φ is the direction angle, and h is the height of the center of gravity from the ground. is the steering resistance coefficient when turning on a slope, and J is the moment of inertia; the above variables are constant values ​​or values ​​that can be calculated with constant coefficients;

[0124] in, , are the driving resistance of the ground acting on the track, M is the steering resistance torque of the ground acting on the track, and the three are values ​​that change with θ and φ;

[0125] in, and are the driving force / braking force transmitted from the driving wheels on both sides to the track, which are the variables that actually perform the control function of this method; and When it is a positive value, it means that the output driving force should be controlled at this time. and When it is a negative value, it means that the output braking force should be controlled at this time;

[0126] Where v is the speed of the tracked vehicle, pointing to the front of the vehicle; ω is the steering angular velocity, that is, the rate of change of the heading angle α;

[0127] S5032. Discretize the motion state equation, and we have:

[0128] ;

[0129] ;

[0130] The discrete inference relationship between the position and heading angle of the vehicle calculated according to the discrete equation is:

[0131] ;

[0132] S5033, based on the above discrete relationship, the speed, steering angular velocity and position at the next moment under the action of a specific driving / braking force can be estimated according to the speed, steering angular velocity and position at the previous moment;

[0133] It should be noted that since z, β and γ are usually determined by the actual terrain, that is, when the tracked vehicle travels to the terrain, if the tracked vehicle is in close contact with the ground, x, y, α can determine the corresponding values ​​of z, β, γ. The tracked vehicle motion control system is a typical under-actuated system.

[0134] S600, set constraints:

[0135] In this step, the design constraints for driving force / braking force are set as follows:

[0136] ;

[0137] It should be noted that this condition is used to limit the amplitude and change rate of the driving force / braking force to be bounded, and the parameters are determined according to the actual vehicle conditions. , , and The left and right sides are usually equivalent.

[0138] S700, set the objective function:

[0139] In this step, the performance optimization index and optimization direction are set, and the objective function is set according to the optimization index and optimization direction;

[0140] Specifically, since the control goal of the actual vehicle is to enable the tracked vehicle to track the reference trajectory waypoints, the performance optimization indicators and optimization directions are set as follows: (1) the position tracking deviation should be small; (2) the heading angle deviation should be small; (3) the input control signal change rate should be small;

[0141] Specifically, the objective function is as follows:

[0142] ;

[0143] in, To adjust the parameters affecting the weights of the four sub-goals;

[0144] Where N is the number of cycles of predictive control;

[0145] Among them, the above parameters are optimized according to the actual vehicle conditions;

[0146] in, The algorithm is to traverse Waypoints and find the waypoint Wi such that The minimum Wi is The corresponding .

[0147] S800, build model predictive controller MPC:

[0148] In this step, the model predictive controller MPC is called to solve the optimal control value based on the aforementioned prediction model, constraints and objective function;

[0149] It should be noted that the steps for solving the optimal control value are conventional technical means of those skilled in the art, which are similar to the solution process of the nonlinear controller and are not the focus of the present application, and will not be described in detail here.

[0150] Specifically, the optimal control sequence is finally obtained as:

[0151] = ;

[0152] Specifically, the first term of the optimal control sequence is obtained As the control value at the current moment, it is sent to the drive unit / brake unit for execution.

[0153] At this point, the motion control of this cycle is completed, the tracked vehicle will perform controlled motion, and then return to step S200 to start the next cycle control. In the above steps, S200~S800 are for each motion control cycle of each vehicle.

[0154] It should be noted that due to the various power transmission forms of unmanned tracked vehicles, including zero-differential electric transmission, dual-sided independent drive, dual-sided motor coupling drive and three-motor drive, etc., the actual motion control components are not unified, but they will ultimately act on the tracks on both sides through the left and right driving wheels; this is also the reason why this application discusses the driving force / braking force of the left and right tracks as control variables.

[0155] It should be noted that all the above examples are only for explaining the present invention and cannot limit the protection scope of the present invention.

[0156] Embodiment 2:

[0157] This embodiment is based on the same inventive concept as the motion control method for an unmanned tracked vehicle on a multi-directional slope section described in Embodiment 1, and provides a motion control system for an unmanned tracked vehicle on a multi-directional slope section, such as Figure 7 As shown, including:

[0158] Navigation unit, LiDAR, positioning unit, vehicle status monitoring unit, motion control unit, and drive unit / brake unit;

[0159] Among them, the navigation unit is used to output the reference trajectory of the unmanned tracked vehicle, and then guide the unmanned tracked vehicle to follow the reference trajectory. Usually, this reference trajectory has taken into account the anti-collision and obstacle avoidance functions and is an ideally achievable trajectory.

[0160] Among them, lidar is used to provide point cloud information to detect the terrain of the road ahead and calculate the slope.

[0161] Among them, the positioning unit is used to provide the vehicle's current spatial coordinate information, including: three-dimensional position and three-dimensional turning angles, etc.

[0162] Among them, the vehicle status monitoring unit is used to provide vehicle status information, including: the actual driving force / braking force of the left and right tracks, etc.

[0163] Among them, the motion control unit is actually equipped with the motion control method of the present invention, and is used to execute and realize the control strategy in the control method of Example 1.

[0164] Among them, the driving unit / braking unit is used to distribute the driving force / braking force of the left and right driving wheels, and finally acts on the left and right tracks to interact with the ground.

[0165] Specifically, under normal circumstances, the laser radar is installed above the front of the vehicle, and the positioning unit is installed at the center of mass of the vehicle. Both can be installed and calibrated according to the common specifications of unmanned vehicles.

[0166] It should be noted that all the above examples are only for explaining the present invention and cannot limit the protection scope of the present invention.

[0167] Embodiment 3:

[0168] This embodiment provides a computer-readable storage medium, including:

[0169] The storage medium is used to store computer software instructions used to implement the motion control method for an unmanned tracked vehicle on a multi-directional slope section described in the above-mentioned embodiment 1, which includes a program for executing the above-mentioned program set for the motion control method for an unmanned tracked vehicle on a multi-directional slope section; specifically, the executable program can be built into the motion control system of the unmanned tracked vehicle on a multi-directional slope section described in embodiment 2, so that the motion control system of the unmanned tracked vehicle on a multi-directional slope section can implement the motion control method for an unmanned tracked vehicle on a multi-directional slope section described in embodiment 1 by executing the built-in executable program.

[0170] In addition, the computer-readable storage medium of this embodiment may adopt any combination of one or more computer-readable storage media, wherein the computer-readable storage medium includes electrical, optical, electromagnetic, infrared or semiconductor systems, devices or components, or any combination thereof.

[0171] Different from the prior art, the motion control method of an unmanned tracked vehicle on a multi-directional slope section of the present application can be based on advanced sensors, control algorithms and vehicle dynamics prediction control models. When the tracked vehicle is on a slope section with complex undulations all around, the slope angle of the terrain that the vehicle may encounter in the future can be predicted and the optimal control sequence can be calculated. Finally, the driving force or braking force of the tracks on both sides of the vehicle can be optimized according to the optimal control sequence, so as to achieve stable control of the vehicle speed and steering angular velocity, achieve stable tracking of the reference trajectory, improve the stability of the vehicle, avoid adverse situations such as forced stopping, sliding down the slope and running off the road, improve the vehicle's execution efficiency and execution safety for the task, make up for the defects of the prior art, and have high application value.

[0172] It should be understood that in the various embodiments of this document, the size of the serial numbers of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this document.

[0173] It should also be understood that in the embodiments of this article, the term "and / or" is only a description of the association relationship of the associated objects, indicating that three relationships may exist. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship.

[0174] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this article.

[0175] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0176] In the several embodiments provided herein, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic, for example, the division of the units is only a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, or can be electrical, mechanical or other forms of connection.

[0177] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the embodiments of this article.

[0178] In addition, each functional unit in each embodiment of this invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above integrated unit may be implemented in the form of hardware or in the form of software functional unit.

[0179] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this article is essentially or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of this article. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0180] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A motion control method for an unmanned tracked vehicle on a multi-directional slope section, characterized in that: The following steps are involved: Initialization steps: Establishing a geodetic coordinate system; acquiring a vehicle reference trajectory, vehicle position information, and original point cloud information of a laser radar; and converting the original point cloud information into complete point cloud information in the geodetic coordinate system based on the geodetic coordinate system and the vehicle position information; Vehicle control steps: Constructing a prediction model, wherein the prediction model is used to predict the vehicle's expected arrival position based on the vehicle's current position and status; Setting constraints and an objective function; calling a model prediction controller to calculate an optimal control sequence based on the prediction model, the constraints and the objective function; calling an execution unit of the vehicle to control the driving force or braking force of the vehicle according to the optimal control sequence; The prediction logic of the prediction model for the vehicle's expected arrival position includes: confirming the vehicle's current position in the complete point cloud information; calculating the plane normal vector at the vehicle's current position; calculating the equivalent slope angle at the vehicle's current position and the direction angle of the vehicle traveling on the ramp based on the plane normal vector; determining the vehicle's motion state equation based on the vehicle's current position, the equivalent slope angle and the direction angle; and predicting the vehicle's expected arrival position based on the motion state equation.

2. The motion control method for an unmanned tracked vehicle on a multi-directional slope section according to claim 1, characterized in that: The calculating of the plane normal vector at the current position of the vehicle comprises: In the complete point cloud information, a point cloud subset is selected to represent the vehicle attachment condition at the current position of the vehicle; Calculating a fitted plane equation and a covariance matrix for the point cloud subset; Determining the optimal condition when fitting the plane according to the covariance matrix; The normal vector of the fitting plane is determined according to the optimal condition as the plane normal vector at the current position of the vehicle.

3. The motion control method for an unmanned tracked vehicle on a multi-directional slope section according to claim 1, characterized in that: The step of calculating the equivalent slope angle at the current position of the vehicle and the direction angle of the vehicle traveling on the slope according to the plane normal vector includes: Substituting the dot product between the normal vector of the horizontal plane and the plane normal vector at the current position of the vehicle into the cosine value formula of the angle between the horizontal plane and the fitting plane, and calculating the equivalent slope angle by arc cosine; The heading angle of the vehicle is subtracted from the angle between the projection of the plane normal vector at the current position of the vehicle on the horizontal plane and the X-axis, and then subtracted from π to obtain the direction angle.

4. The motion control method for an unmanned tracked vehicle on a multi-directional slope section according to claim 1, characterized in that: The motion state equation includes: In the motion state equation, G is the vehicle weight, m ​​is the vehicle mass, L is the track ground length, B is the track center distance, θ is the slope angle, φ is the direction angle, h is the height of the center of gravity from the ground, μ is the steering resistance coefficient when turning on the slope, and J is the moment of inertia; In the motion state equation, R1 and R2 are respectively the driving resistance of the ground acting on the vehicle track, M is the steering resistance torque of the ground acting on the vehicle track, and R1, R2 and M are values ​​that change with θ and φ; In the motion state equation, F1 and F2 are the driving force or braking force transmitted to the track by the driving wheels on both sides of the vehicle; In the motion state equation, v is the vehicle speed, which points to the front of the vehicle; ω is the steering angular velocity, which is the rate of change of the vehicle heading angle α.

5. The motion control method for an unmanned tracked vehicle on a multi-directional slope section according to claim 1, characterized in that: The predicting the expected arrival position of the vehicle according to the motion state equation includes: The motion state equation is discretized to obtain a discrete equation: Calculating a discrete estimated relationship between the position and heading angle of the vehicle according to the discrete equation; The vehicle speed, vehicle steering angular velocity and vehicle position at the next moment under the set driving force or braking force are predicted based on the discrete estimated relationship, the vehicle speed, vehicle steering angular velocity and vehicle position at the previous moment.

6. The motion control method for an unmanned tracked vehicle on a multi-directional slope section according to claim 1, characterized in that: The constraints include: The constraint condition is a constraint condition on driving force or braking force.

7. The motion control method for an unmanned tracked vehicle on a multi-directional slope section according to claim 1, characterized in that: The objective function includes: In the objective function, λ1, λ2, λ3 and λ4 are parameters for adjusting the influence weights of the four sub-objectives; In the objective function, N is the number of cycles of predictive control; In the objective function, Veh ref The algorithm is to traverse the vehicle reference trajectory and find the waypoints Wi such that The smallest Wi is Veh k The corresponding Veh ref ;Veh k is the current vehicle position.

8. The motion control method for an unmanned tracked vehicle on a multi-directional slope section according to claim 1, characterized in that: The optimal control sequence comprises: F * ={(F * 1,0 ,F * 2,0 ),...,(F * 1,N-1 ,F * 2,N-1 )}。 9. The motion control method for an unmanned tracked vehicle on a multi-directional slope section according to claim 8, characterized in that: The step of calling the execution unit of the vehicle to control the driving force or braking force of the vehicle according to the optimal control sequence includes: The first item (F * 1,0 ,F * 2,0 ) is used as the control value at the current moment and sent to the drive unit or brake unit to execute the driving force or braking force.