A transverse-longitudinal coupling control method and system for a downhole shovel based on iLQR

Abstract

This invention provides a lateral and longitudinal coupling control method and system for underground loaders based on iLQR, belonging to the field of autonomous walking control technology for articulated mining loaders. Firstly, by constructing a unified kinematic model and cost function, this invention solves the accuracy loss and stability problems caused by neglecting system coupling effects in traditional decoupling methods. Secondly, it possesses full-condition adaptive capability for forward / reverse movement, using the driving direction to guide the vehicle's state as the optimization objective, effectively addressing the industry pain point of poor tracking performance in different directions. Finally, by introducing a smooth input saturation mapping and an exponential barrier function, safety constraints are seamlessly embedded into the optimization process, ensuring not only the physical feasibility of control commands but also actively preventing vehicle instability, thus improving the inherent safety and solution stability of the system. Simultaneously, by employing the efficient iLQR algorithm as the core solver, complex nonlinear optimization is transformed into a series of rapidly solvable sub-problems, significantly reducing the computational load.

Description

Technical Field

[0001] This invention relates to the field of autonomous walking control technology for articulated mining loaders, and particularly to a horizontal and vertical coupling control method and system for underground loaders based on iLQR. Background Technology

[0002] Articulated vehicles connect their front and rear frames via an articulation mechanism, and steering is achieved by changing the articulation angle. They offer significant advantages such as strong terrain adaptability, high flexibility, and a small turning radius. Therefore, articulated vehicles are widely used in mining transportation, tunnel construction, and operations in complex terrain. Especially in narrow roads, winding tunnels, or confined spaces, their unique steering mechanism allows for efficient turning and path adjustment within limited spaces, making them an important technical solution for heavy-duty, low-speed work vehicles.

[0003] However, articulated vehicles face significant challenges in autonomous movement control, especially in the complex underground environment. Underground tunnels typically feature narrow spaces, low visibility, and uneven surfaces. Combined with interference factors such as dust and moisture in the mining environment, precise control of articulated vehicles becomes exceptionally difficult. While traditional separate lateral and longitudinal control methods are widely used in path-following tasks, they often fail to effectively coordinate speed, steering, and articulation angles due to insufficient consideration of the nonlinear dynamics of the articulated vehicle. Particularly during steering, the relative rotation of the front and rear bodies induces changes in the center of gravity, yaw rate, and lateral slip characteristics, causing lateral and longitudinal motions to couple, making it difficult to guarantee control effectiveness. Summary of the Invention

[0004] To address the problems in the existing technologies, this invention provides a lateral and longitudinal coupling control method and system for underground loader operators based on iLQR. Firstly, by constructing a unified kinematic model and cost function, this invention solves the accuracy loss and stability issues caused by neglecting system coupling effects in traditional decoupling methods, significantly improving trajectory tracking performance under complex working conditions. Secondly, it possesses full-condition adaptive capability for forward / reverse movement, using the driving direction to guide the vehicle's state as the optimization objective, effectively addressing the industry pain point of poor tracking performance in different directions and achieving high-performance control throughout the entire working cycle. Finally, by introducing a smooth input saturation mapping and an exponential barrier function, safety constraints are seamlessly embedded into the optimization process, ensuring not only the physical feasibility of control commands but also actively preventing vehicle instability, improving the inherent safety and solution stability of the system. Simultaneously, the efficient iLQR algorithm is used as the core solver, transforming complex nonlinear optimization into a series of rapidly solvable sub-problems, significantly reducing computational load and meeting the real-time requirements of industrial sites. To achieve the above objectives, the technical solution is as follows:

[0005] On one hand, this invention provides a lateral and longitudinal coupling control method for downhole loaders based on iLQR, the method comprising: S1. Based on the underground loader, build a unified kinematic structure model of the articulated vehicle; S2. Based on the movement path of the underground loader, construct a reference trajectory centered on the vehicle body; S3. Based on the unified kinematic structure model of the articulated vehicle, the constraint model of the articulated vehicle is obtained through constraint processing based on input mapping and state barrier function. S4. Based on the constraint model of the articulated vehicle, by designing the coupling cost function and safety cost function that integrate the lateral and longitudinal control objectives, the motion objective function of the articulated vehicle is obtained. S5. Based on the motion objective function of the articulated vehicle, the unified kinematic structure model of the articulated vehicle, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader, the iLQR algorithm is used for iterative optimization to obtain the coupled control command. S6. According to the coupling control command, the controller executes the coupling control command and collects the real-time position of the underground loader to obtain the new state of the underground loader after movement; S7. Replace the vehicle state of the underground loader with the new state after its movement, and repeat steps S5 to S6 to achieve continuous control of the underground loader.

[0006] Optionally, in S1, a unified kinematic structure model of the articulated vehicle is built based on the underground loader, including: S11. Based on the underground loader, obtain the motion relationship model of the front and rear bodies of the articulated vehicle; S12. Based on the motion relationship model of the front and rear bodies of the articulated vehicle, define the state vector and control vector of the guide body in the underground loader to obtain the unified kinematic structure model of the articulated vehicle.

[0007] Optionally, the judgment rules for the guide vehicle body include: Rule 1: When the underground loader is moving forward, the front body of the underground loader shall be selected as the guide body; Rule 2: When the underground loader is traveling in reverse, the rear body of the underground loader shall be selected as the guide body.

[0008] Optionally, in S3, based on the unified kinematic structure model of the articulated vehicle, a constraint model of the articulated vehicle is obtained through constraint processing based on input mapping and state barrier functions, including: S31. Based on the unified kinematic structure model of the articulated vehicle, and the control quantity range of the model is obtained based on the control quantity constraints of the input saturation mapping. S32. Based on the unified kinematic structure model of the articulated vehicle, the state quantity constraints are handled using the obstacle barrier function to obtain the state quantity interval of the model. S33. By constraining the unified kinematic structure model of the articulated vehicle through the control variable range and the state variable range of the model, the constraint model of the articulated vehicle is obtained.

[0009] Optionally, in S4, based on the constraint model of the articulated vehicle, the motion objective function of the articulated vehicle is obtained by designing a coupled cost function that integrates the lateral and longitudinal control objectives and a safety cost function, including: S41. Based on the constraint model of the articulated vehicle, the lateral and longitudinal control objectives are fused to obtain the coupling cost function of the fused lateral and longitudinal control objectives. S42. Based on the constraint model of the articulated vehicle, set safety constraints and obtain the safety cost function; S43. Based on the coupling cost function of the fused lateral and longitudinal control objectives and the safety cost function, the motion objective function of the articulated vehicle is obtained.

[0010] Optionally, in S5, based on the articulated vehicle's objective motion function, its unified kinematic structure model, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader, the iLQR algorithm is used for iterative optimization to obtain coupled control commands, including: S51. Based on the unified kinematic structure model of the articulated vehicle, and by inputting the initialization control sequence and the vehicle state of the underground loader, the future motion trajectory and state of the vehicle are obtained. S52. Substitute the future vehicle's trajectory and state, along with the reference trajectory centered on the vehicle body, into the articulated vehicle's motion objective function, and use iLQR's back-calculation to solve for the optimized control sequence as the new initial control sequence. S53. Repeat steps S51 to S52 until the iteration terminates, and select the first value of the optimized control sequence as the coupling control command.

[0011] Optionally, in S6, according to the coupling control command, the controller executes the coupling control command and collects the real-time position of the downhole loader to obtain the new state of the downhole loader after its movement, including: S61. Based on the coupling control command, the longitudinal acceleration command and steering angular velocity command are obtained by processing through hyperbolic tangent saturation mapping. S62. Based on the longitudinal acceleration command and the steering angle velocity command, the PID controller is fed into the controller to obtain the controller command, and the controller command is sent to the vehicle controller via the bus. S63. According to the controller instruction, control the movement of the underground loader and collect data to obtain the new state of the underground loader after movement.

[0012] On the other hand, the present invention provides a lateral and longitudinal coupling control system for an iLQR-based downhole loader. This system is applied to a lateral and longitudinal coupling control method for an iLQR-based downhole loader, and includes: The model building module is used to build a unified kinematic structure model of the articulated vehicle based on the underground loader. The trajectory generation module is used to construct a reference trajectory centered on the vehicle body based on the movement path of the underground loader; The constraint construction module is used to obtain the constraint model of the articulated vehicle by means of constraint processing based on input mapping and state barrier function, according to the unified kinematic structure model of the articulated vehicle. The objective function generation module is used to obtain the motion objective function of the articulated vehicle by designing a coupled cost function that integrates the lateral and longitudinal control objectives and a safety cost function, based on the constraint model of the articulated vehicle. The control command acquisition module is used to obtain coupled control commands by iteratively optimizing the iLQR algorithm based on the motion objective function of the articulated vehicle, the unified kinematic structure model of the articulated vehicle, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader. The motion execution module is used to execute the coupled control command through the controller and collect the real-time position of the downhole loader according to the coupled control command, so as to obtain the new state of the downhole loader after the motion. The continuous control module is used to replace the vehicle state of the underground loader with the new state after its movement, and repeat the above steps to achieve continuous control of the underground loader.

[0013] Compared with the prior art, the technical solution of the present invention has at least the following beneficial effects: The above-mentioned solution addresses several key aspects. First, by constructing a unified kinematic model and cost function, it resolves the accuracy and stability issues caused by neglecting system coupling effects in traditional decoupling methods, significantly improving trajectory tracking performance under complex conditions. Second, it possesses full-condition adaptive capability for both forward and reverse driving, using the driving direction to guide the vehicle's state as the optimization objective, effectively addressing the industry pain point of poor tracking performance in different directions and achieving high-performance control throughout the entire working cycle. Third, by introducing smooth input saturation mapping and an exponential barrier function, it seamlessly embeds safety constraints into the optimization process, ensuring not only the physical feasibility of control commands but also proactively preventing vehicle instability, thus enhancing the inherent safety and solution stability of the system. Fourth, by employing the efficient iLQR algorithm as the core solver, it transforms complex nonlinear optimization into a series of rapidly solvable sub-problems, significantly reducing computational load and meeting the real-time requirements of industrial environments. Attached Figure Description

[0014] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0015] Figure 1 This is a flowchart of an embodiment of the iLQR-based longitudinal and lateral coupling control method for downhole loaders of the present invention; Figure 2 This is a flowchart of building a unified kinematic structure model of an articulated vehicle in an embodiment of the iLQR-based longitudinal and lateral coupling control method for downhole loaders of the present invention. Figure 3 This is a flowchart of the constraint model of the articulated vehicle obtained in an embodiment of the iLQR-based longitudinal and lateral coupling control method for downhole loaders of the present invention. Figure 4 This is a flowchart of the motion objective function of the articulated vehicle obtained in an embodiment of the iLQR-based longitudinal and lateral coupling control method for downhole loaders of the present invention; Figure 5 This is a flowchart of the coupling control command obtained in an embodiment of the iLQR-based horizontal and vertical coupling control method for downhole loaders of the present invention; Figure 6 This is a flowchart illustrating the new state of the downhole loader after its movement, obtained in an embodiment of the iLQR-based longitudinal and lateral coupling control method for downhole loaders of the present invention. Figure 7 This is a comparison chart of the angular velocities of the L-shaped trajectory tracking test between the articulated vehicle autonomous walking control method based on lateral-longitudinal coupling and the method not based on lateral-longitudinal coupling, provided in the embodiment of the iLQR-based lateral-longitudinal coupling control method for underground loaders of the present invention. Figure 8 This is a comparison chart of acceleration tests for L-shaped trajectory tracking tests between the articulated vehicle autonomous walking control method based on lateral-longitudinal coupling and the method not based on lateral-longitudinal coupling, provided in an embodiment of the iLQR-based lateral-longitudinal coupling control method for underground loaders of the present invention. Figure 9 This is a system block diagram of an embodiment of the horizontal and vertical coupling control system for an downhole loader based on iLQR of the present invention. Detailed Implementation

[0016] The technical solution of the present invention will now be described with reference to the accompanying drawings.

[0017] In embodiments of the present invention, words such as "exemplarily," "for example," etc., are used to indicate that something is an example, illustration, or description. Any embodiment or design described as "exemplary" in the present invention should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of the word "exemplary" is intended to present the concept in a concrete manner. Furthermore, in embodiments of the present invention, the meaning expressed by "and / or" can be both, or either one.

[0018] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with the accompanying drawings and specific embodiments.

[0019] like Figure 1 The flowchart shown is an embodiment of the iLQR-based lateral and longitudinal coupling control method for an underground loader according to the present invention. The present invention provides an iLQR-based lateral and longitudinal coupling control method for an underground loader, which is implemented by an iLQR-based lateral and longitudinal coupling control system for the underground loader. The method includes: S1. Based on the underground loader, build a unified kinematic structure model of the articulated vehicle; Specifically, such as Figure 2 The flowchart shown in this embodiment of the iLQR-based longitudinal and lateral coupling control method for underground loader of the present invention illustrates the process of building a unified kinematic structure model of the articulated vehicle. In step S1, building the unified kinematic structure model of the articulated vehicle based on the underground loader includes: S11. Based on the underground loader, obtain the motion relationship model of the front and rear bodies of the articulated vehicle; Furthermore, it is known that an underground loader is a special vehicle structure consisting of a front body and a rear body connected by a central hinge point. Kinematically, this structure can be abstracted as a model of two rigid bodies (front and rear bodies) connected by a rotary hinge joint. The front and rear bodies are two independent rigid body units, achieving relative rotation through the central hinge joint; the vehicle's movement and steering depend on the interaction between the wheel systems of the front and rear bodies and the ground.

[0020] (1) Among them, subscript Indicates the rear of the vehicle. Indicates the front of the vehicle; and These are the linear velocities of the front and rear vehicle bodies, respectively. and The rate of change of the heading angle of the front and rear vehicle bodies. and These represent the distances from the articulation point to the center of gravity of the front and rear vehicle bodies, respectively.

[0021] S12. Based on the motion relationship model of the front and rear bodies of the articulated vehicle, define the state vector and control vector of the guide body in the underground loader to obtain the unified kinematic structure model of the articulated vehicle.

[0022] Furthermore, in order to uniformly describe the overall motion state of the vehicle, this disclosure defines the state vector of the guiding vehicle body as follows: (2) in, and To guide the vehicle's position coordinates in the global coordinate system, Indicates the heading angle of the guiding vehicle. To guide the linear velocity of the vehicle body, This refers to the articulation angle of the articulated vehicle.

[0023] And define the control input vector for the downhole loader: (3) in, Indicates the vehicle's acceleration. For the turning speed of the loader, acceleration and turning angular velocity are selected as control variables.

[0024] After defining the state vector of the guiding vehicle, this disclosure establishes a unified continuous-time kinematic model based on the structural characteristics of articulated vehicles. This model, using the guiding vehicle as a reference, uniformly represents the vehicle's lateral motion (position, heading, articulation angle) and longitudinal motion (velocity, acceleration), and is applicable to both forward and reverse driving modes. Specifically, the motion of the guiding vehicle is determined by the linear velocity, the rate of change of the heading angle, and the rate of change of the articulation angle. Furthermore, the nonlinear kinematic equations between the front and rear vehicle bodies can be expressed through the articulation angle... The derivation is performed, where the lateral velocity component is coupled with the heading angle, and the rate of change of heading is affected by the articulation angle attitude and the speed of the guide vehicle, thus comprehensively reflecting the nonlinear coupling behavior of the articulated vehicle. To facilitate trajectory prediction, gradient calculation, and optimal control solutions within the iLQR framework, the aforementioned continuous-time unified kinematic model needs to be discretized. Let the sampling time be... By discretizing the state equations using the first-order forward Euler method, a unified kinematic structure model of the articulated vehicle can be obtained:

[0025] (4) in, Indicates the first The state vector of the guiding vehicle at any given time. and These are the longitudinal acceleration and hinge angular velocity control inputs, respectively. and These represent the distances from the center of gravity of the vehicle body to the articulation point and the distances from the center of gravity of the vehicle body to the articulation point, respectively. To accommodate both forward and reverse driving conditions, this disclosure introduces symbolic variables. Used to unify model representation: when the vehicle is moving forward When the vehicle is reversing This symbolic variable enables indiscriminate modeling of forward and reverse travel, allowing the rate of change of heading and steering dynamics related to the articulation angle to be represented by the same mathematical structure under both operating conditions.

[0026] Furthermore, the rules for determining the guiding vehicle body include: Rule 1: When the underground loader is moving forward, the front body of the underground loader shall be selected as the guide body; Rule 2: When the underground loader is traveling in reverse, the rear body of the underground loader shall be selected as the guide body.

[0027] S2. Based on the movement path of the underground loader, construct a reference trajectory centered on the vehicle body; Specifically, based on the movement path of the downhole loader, in order to obtain a virtual reference point in the future prediction time domain, this invention utilizes the vehicle's current position. Heading angle and the vehicle's current speed The prediction is made point-by-point along the vehicle's current orientation over several future steps. The virtual position of a step can be represented as

[0028] (5) in, Indicates vehicle number The virtual point of the step, The discrete-time sampling period is , where is the step size index in the prediction time domain. The above formula generates virtual reference trajectory points distributed along the vehicle's current orientation by extrapolating the vehicle's current attitude.

[0029] Subsequently, to ensure that the predicted trajectory remains consistent with the movement path of the downhole loader, this invention performs a process for each virtual point. Find its nearest point in the real global reference trajectory. And use the nearest point as the prediction time domain. The reference trajectory of the step.

[0030] S3. Based on the unified kinematic structure model of the articulated vehicle, the constraint model of the articulated vehicle is obtained through constraint processing based on input mapping and state barrier function. Specifically, such as Figure 3The flowchart shown in the embodiment of the iLQR-based longitudinal and lateral coupling control method for underground loaders of the present invention obtains the constraint model of the articulated vehicle. In step S3, based on the unified kinematic structure model of the articulated vehicle, the constraint model of the articulated vehicle is obtained through constraint processing based on input mapping and state barrier functions, including: S31. Based on the unified kinematic structure model of the articulated vehicle, and the control quantity range of the model is obtained based on the control quantity constraints of the input saturation mapping. Furthermore, this embodiment employs a smooth, differentiable input mapping function to map the original control variables. Mapped to actual executable control variables This allows for a natural embedding of the control limit. The mapping formula is as follows:

[0031] (6) in, and These represent the maximum and minimum permissible values ​​of the control variable, respectively. The scaling vector is used to control the slope of the saturation function. This indicates element-wise division.

[0032] S32. Based on the unified kinematic structure model of the articulated vehicle, the state quantity constraints are handled using the obstacle barrier function to obtain the state quantity interval of the model. Furthermore, this embodiment employs an exponential barrier function to apply soft penalties to state constraints. When the system state is within the safe range, the barrier penalty remains a small, constant value; when the state approaches or exceeds the constraint boundary, the penalty increases rapidly, thereby forcing the system away from dangerous areas during the optimization process. The expression for the barrier function is:

[0033] (7) in, , indicating the first A constraint function, To adjust the parameters of the penalty scale and growth rate, this obstacle function can provide effective constraints on the vehicle's articulation angle and speed while ensuring that the cost function is reasonable.

[0034] S33. By constraining the unified kinematic structure model of the articulated vehicle through the control variable range and the state variable range of the model, the constraint model of the articulated vehicle is obtained.

[0035] S4. Based on the constraint model of the articulated vehicle, by designing the coupling cost function and safety cost function that integrate the lateral and longitudinal control objectives, the motion objective function of the articulated vehicle is obtained. Specifically, such as Figure 4The flowchart shown in the embodiment of the iLQR-based lateral and longitudinal coupling control method for downhole loaders of the present invention illustrates the motion objective function of the articulated vehicle. In step S4, based on the constraint model of the articulated vehicle, the motion objective function of the articulated vehicle is obtained by designing a coupling cost function that integrates the lateral and longitudinal control objectives and a safety cost function, including: S41. Based on the constraint model of the articulated vehicle, the lateral and longitudinal control objectives are fused to obtain the coupling cost function of the fused lateral and longitudinal control objectives. Furthermore, based on the constraint model of the articulated vehicle, a cost function that simultaneously penalizes lateral and longitudinal errors is established. : (8) The functions and coupling effects of the aforementioned cost function are as follows: and This is the position error of the guide vehicle, which directly reflects the lateral tracking accuracy. Optimizing this requires the controller to coordinate steering and speed simultaneously so that the guide vehicle closely follows the reference path. The vehicle heading error term guides the vehicle's orientation to ensure that the vehicle's orientation is consistent with the tangential direction of the desired path, and together with the position error term, constitutes the core objective of lateral control. and The first two terms are the longitudinal speed error term and the articulation angle penalty term for the articulated vehicle; the latter two are control quantity penalty terms, which not only ensure control smoothness and comfort, but also affect the response characteristics of the lateral and longitudinal systems, suppressing oversteer during longitudinal acceleration by adjusting parameters; the rest... These are the weighting parameters for each penalty.

[0036] S42. Based on the constraint model of the articulated vehicle, set safety constraints and obtain the safety cost function; Furthermore, an exponential barrier function for the hinge angle is introduced into the cost function to construct the final safety cost function. : (9) in, The safe threshold for working at the hinge angle. This is the adjustment coefficient.

[0037] S43. Based on the coupling cost function of the fused lateral and longitudinal control objectives and the safety cost function, the motion objective function of the articulated vehicle is obtained.

[0038] S5. Based on the motion objective function of the articulated vehicle, the unified kinematic structure model of the articulated vehicle, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader, the iLQR algorithm is used for iterative optimization to obtain the coupled control command. Specifically, such as Figure 5The flowchart shown in this embodiment of the iLQR-based longitudinal and lateral coupling control method for an underground loader according to the present invention obtains the coupling control command. In step S5, based on the motion objective function of the articulated vehicle, the unified kinematic structure model of the articulated vehicle, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader, the coupling control command is obtained through iterative optimization using the iLQR algorithm, including: S51. Based on the unified kinematic structure model of the articulated vehicle, and by inputting the initialization control sequence and the vehicle state of the underground loader, the future motion trajectory and state of the vehicle are obtained. S52. Substitute the future vehicle's trajectory and state, along with the reference trajectory centered on the vehicle body, into the articulated vehicle's motion objective function, and use iLQR's back-calculation to solve for the optimized control sequence as the new initial control sequence. S53. Repeat steps S51 to S52 until the iteration terminates, and select the first value of the optimized control sequence as the coupling control command.

[0039] Furthermore, the iterative process of the iLQR algorithm begins with a forward simulation. Specifically, at the start of a control cycle, based on a preset initial control sequence... Based on the current vehicle status Starting from this point, a forward simulation is performed in the prediction time domain, that is, a nominal state trajectory is obtained by recursively calculating using the dynamic model from steps 2-3. Subsequently, along this nominal trajectory, at each time point... We calculate the Jacobian matrix of the dynamic model with respect to the state and control, as well as the Hessian matrix of the cost function with respect to the state and control, to facilitate subsequent iLQR calculations.

[0040] Starting from the end of the prediction time domain, the augmented cost function is quadratized point by point along the nominal trajectory, and the dynamic model is linearized. Then, by solving a series of time-varying Riccati-like equations, the feedback gain matrix used to correct the intermediate control variables is calculated. and feedforward terms : (10) in, These are the control vectors of the cost function. State vector The second-order matrix and first-order partial derivatives of the given matrix are used to determine the gradient, which combines the immediate cost of the current control with its indirect impact on all future time points. Based on the calculated feedback law, the algorithm enters the iterative optimization phase, refining and evaluating the intermediate control variable sequence. First, an initial search step size is set. A new sequence of candidate intermediate control variables is generated using the feedback law, and its update rule is as follows:

[0041] (11) in, This represents the state deviation. Then, this candidate sequence... Repeat the forward simulation process to obtain a new total cost. .Will With the old price Comparison: If If the candidate sequence is correct, the update is accepted, and the candidate sequence is used as the new nominal sequence for the next iteration; otherwise, the sequence is updated according to the preset factor. Reduce the search step size and retry. If If the cost does not decrease after being reduced to the threshold, the update is abandoned. This iterative cycle continues until the convergence condition is met or the maximum number of iterations is reached. Finally, after the iteration ends, the first element is extracted from the sequence of intermediate control variables obtained from the optimization. And use that as the final output of this step.

[0042] S6. According to the coupling control command, the controller executes the coupling control command and collects the real-time position of the underground loader to obtain the new state of the underground loader after movement; Specifically, such as Figure 6 The flowchart shown in the embodiment of the iLQR-based longitudinal and lateral coupling control method for downhole loaders of the present invention obtains the new state of the downhole loader after movement. In step S6, according to the coupling control command, the controller executes the coupling control command and collects the real-time position of the downhole loader to obtain the new state of the downhole loader after movement, including: S61. Based on the coupling control command, the longitudinal acceleration command and steering angular velocity command are obtained by processing through hyperbolic tangent saturation mapping. S62. Based on the longitudinal acceleration command and the steering angle velocity command, the PID controller is fed into the controller to obtain the controller command, and the controller command is sent to the vehicle controller via the bus. S63. According to the controller instruction, control the movement of the underground loader and collect data to obtain the new state of the underground loader after movement.

[0043] Specifically, such as Figure 7 The following is a comparison of the angular velocities of the articulated vehicle autonomous walking control method based on lateral-longitudinal coupling and the L-shaped trajectory tracking test results provided by the embodiment of the iLQR-based lateral-longitudinal coupling control method for downhole loaders of the present invention, as shown in the figure. Figure 8The diagram shown is a comparison of acceleration in L-shaped trajectory tracking tests between the articulated vehicle autonomous walking control method based on lateral-longitudinal coupling and the method not based on lateral-longitudinal coupling, provided by the embodiment of the iLQR-based lateral-longitudinal coupling control method for underground loaders of the present invention. The comparison results of angular velocity and acceleration in L-shaped trajectory tracking tests between the articulated vehicle autonomous walking control method based on lateral-longitudinal coupling and the method not based on lateral-longitudinal coupling are obtained.

[0044] like Figure 9 The diagram shown is a system block diagram of an embodiment of the iLQR-based lateral and longitudinal coupling control system for an underground loader according to the present invention. The present invention provides a lateral and longitudinal coupling control system for an underground loader based on iLQR. This system is applied to a lateral and longitudinal coupling control method for an iLQR-based underground loader. The system includes: a model building module, a trajectory generation module, a constraint construction module, an objective function generation module, a control command acquisition module, a motion execution module, and a continuous control module. Specifically, The model building module is used to build a unified kinematic structure model of the articulated vehicle based on the underground loader. The trajectory generation module is used to construct a reference trajectory centered on the vehicle body based on the movement path of the underground loader; The constraint construction module is used to obtain the constraint model of the articulated vehicle by means of constraint processing based on input mapping and state barrier function, according to the unified kinematic structure model of the articulated vehicle. The objective function generation module is used to obtain the motion objective function of the articulated vehicle by designing a coupled cost function that integrates the lateral and longitudinal control objectives and a safety cost function, based on the constraint model of the articulated vehicle. The control command acquisition module is used to obtain coupled control commands by iteratively optimizing the iLQR algorithm based on the motion objective function of the articulated vehicle, the unified kinematic structure model of the articulated vehicle, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader. The motion execution module is used to execute the coupled control command through the controller and collect the real-time position of the downhole loader according to the coupled control command, so as to obtain the new state of the downhole loader after the motion. The continuous control module is used to replace the vehicle state of the underground loader with the new state after its movement, and repeat the above steps to achieve continuous control of the underground loader.

[0045] This invention provides a lateral and longitudinal coupling control method and system for underground loader based on iLQR. Firstly, by constructing a unified kinematic model and cost function, it solves the accuracy loss and stability problems caused by neglecting system coupling effects in traditional decoupling methods, significantly improving trajectory tracking performance under complex working conditions. Secondly, it possesses full-condition adaptive capability for forward / reverse movement, using the driving direction to guide the vehicle's state as the optimization objective, effectively addressing the industry pain point of poor tracking performance in different directions and achieving high-performance control throughout the entire working cycle. Finally, by introducing smooth input saturation mapping and an exponential barrier function, safety constraints are seamlessly embedded into the optimization process, ensuring not only the physical feasibility of control commands but also actively preventing vehicle instability, improving the inherent safety and solution stability of the system. Simultaneously, by employing the efficient iLQR algorithm as the core solver, complex nonlinear optimization is transformed into a series of rapidly solvable sub-problems, significantly reducing computational load and meeting the real-time requirements of industrial sites.

[0046] It is understood that the present invention has been described through the above embodiments and should not be construed as limiting the implementation and scope of the present invention. Those skilled in the art will recognize that various changes or equivalent substitutions can be made to these features and embodiments without departing from the spirit and scope of the present invention. Furthermore, under the teachings of the present invention, these features and embodiments can be modified to adapt to specific situations and materials without departing from the spirit and scope of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are within the protection scope of the present invention.

Claims

1. A lateral and longitudinal coupling control method for an downhole loader based on iLQR, characterized in that, The method includes: S1. Based on the underground loader, build a unified kinematic structure model of the articulated vehicle; S2. Based on the movement path of the underground loader, construct a reference trajectory centered on the vehicle body; S3. Based on the unified kinematic structure model of the articulated vehicle, the constraint model of the articulated vehicle is obtained through constraint processing based on input mapping and state barrier function. S4. Based on the constraint model of the articulated vehicle, by designing a coupled cost function that integrates the lateral and longitudinal control objectives and a safety cost function, the motion objective function of the articulated vehicle is obtained. S5. Based on the motion objective function of the articulated vehicle, the unified kinematic structure model of the articulated vehicle, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader, the iLQR algorithm is used for iterative optimization to obtain the coupled control command. S6. According to the coupling control command, the controller executes the coupling control command and collects the real-time position of the underground loader to obtain the new state of the underground loader after movement; S7. Replace the vehicle state of the underground loader with the new state after the movement of the underground loader, and repeat steps S5 to S6 to achieve continuous control of the underground loader. S1 includes: S11. Based on the underground loader, obtain the motion relationship model of the front and rear bodies of the articulated vehicle. （1） in, and These are the linear velocities of the front and rear vehicle bodies, respectively. and The rate of change of the heading angle of the front and rear vehicle bodies. and These are the distances from the hinge point to the center of gravity of the front and rear vehicle bodies, respectively. and The heading angle of the front and rear vehicle bodies. It is the hinge angle; S12. Based on the motion relationship model of the front and rear bodies of the articulated vehicle, define the state vector and control vector of the guide body in the underground loader to obtain the unified kinematic structure model of the articulated vehicle. The state vector for: （2） in, and To guide the vehicle's position coordinates in the global coordinate system, To guide the vehicle's heading angle, To guide the linear velocity of the vehicle body; The control vector for: （3） in, For the acceleration of the loader, This refers to the turning speed of the loader. A unified kinematic structure model for the articulated vehicle can be obtained: （4） in, For the first The state vector of the guiding vehicle at any given time. For the first The state vector of the guiding vehicle at any given time. and These are the longitudinal acceleration and hinge angular velocity control inputs, respectively. For the first The constant guiding angle of the vehicle's heading. For the first The constant guiding linear velocity of the vehicle body, For the first The hinge angle at time, For time intervals, and These are the center of gravity distances from the guiding vehicle body to the articulation point and from the following vehicle body to the articulation point, respectively. As a symbolic variable, when the articulated vehicle is moving forward... When the articulated vehicle reverses .

2. The horizontal and vertical coupling control method for downhole loaders based on iLQR according to claim 1, characterized in that, The rules for determining the guide vehicle body include: Rule 1: When the underground loader is traveling forward, the front body of the underground loader shall be selected as the guide body; Rule 2: When the underground loader is traveling in reverse, the rear body of the underground loader shall be selected as the guide body.

3. The horizontal and vertical coupling control method for downhole loaders based on iLQR according to claim 1, characterized in that, In step S3, based on the unified kinematic structure model of the articulated vehicle, a constraint model of the articulated vehicle is obtained through constraint processing based on input mapping and state barrier functions, including: S31. Based on the unified kinematic structure model of the articulated vehicle, and the control quantity range of the model is obtained based on the control quantity constraint of the input saturation mapping; S32. Based on the unified kinematic structure model of the articulated vehicle, the state quantity constraints are processed using the obstacle barrier function to obtain the state quantity interval of the model; S33. By constraining the unified kinematic structure model of the articulated vehicle through the control variable range and the state variable range of the model, the constraint model of the articulated vehicle is obtained.

4. The horizontal and vertical coupling control method for downhole loaders based on iLQR according to claim 1, characterized in that, In step S4, based on the constraint model of the articulated vehicle, the motion objective function of the articulated vehicle is obtained by designing a coupled cost function that integrates the lateral and longitudinal control objectives and a safety cost function, including: S41. Based on the constraint model of the articulated vehicle, fuse the lateral and longitudinal control objectives to obtain the coupling cost function of the fused lateral and longitudinal control objectives; S42. Based on the constraint model of the articulated vehicle, set safety constraints and obtain the safety cost function; S43. Based on the coupling cost function of the fused lateral and longitudinal control targets and the safety cost function, the motion target function of the articulated vehicle is obtained.

5. The horizontal and vertical coupling control method for downhole loaders based on iLQR according to claim 1, characterized in that, In step S5, based on the articulated vehicle's motion objective function, the articulated vehicle's unified kinematic structure model, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader, the iLQR algorithm is used for iterative optimization to obtain coupled control commands, including: S51. Based on the unified kinematic structure model of the articulated vehicle, the initialization control sequence, and the vehicle state of the underground loader, the future vehicle's motion trajectory and state are obtained. S52. The motion trajectory and state of the future vehicle and the reference trajectory centered on the vehicle body are input into the motion objective function of the articulated vehicle, and the optimized control sequence is obtained by back-calculation of iLQR and used as the new initial control sequence. S53. Repeat steps S51 to S52 until the iteration terminates, and select the first value of the optimized control sequence as the coupling control command.

6. The horizontal and vertical coupling control method for downhole loaders based on iLQR according to claim 1, characterized in that, In step S6, according to the coupling control command, the controller executes the coupling control command and collects the real-time position of the downhole loader to obtain the new state of the downhole loader after its movement, including: S61. Based on the coupling control command, the longitudinal acceleration command and steering angular velocity command are obtained by processing through hyperbolic tangent saturation mapping. S62. The longitudinal acceleration command and the steering angular velocity command are fed into the PID controller to obtain the controller command, and the controller command is sent to the vehicle controller via the bus. S63. According to the controller instructions, control the movement of the underground loader and collect data to obtain the new state of the underground loader after movement.

7. A lateral and longitudinal coupling control system for an downhole loader based on iLQR, used to implement the lateral and longitudinal coupling control method for an downhole loader based on iLQR as described in any one of claims 1-6, characterized in that, The system includes: The model building module is used to build a unified kinematic structure model of the articulated vehicle based on the underground loader. The trajectory generation module is used to construct a reference trajectory centered on the vehicle body based on the movement path of the underground loader; The constraint construction module is used to obtain the constraint model of the articulated vehicle by means of constraint processing based on input mapping and state barrier function, according to the unified kinematic structure model of the articulated vehicle. The objective function generation module is used to obtain the motion objective function of the articulated vehicle by designing a coupling cost function that integrates the lateral and longitudinal control objectives and a safety cost function, based on the constraint model of the articulated vehicle. The control command acquisition module is used to obtain coupled control commands by iteratively optimizing the iLQR algorithm based on the motion target function of the articulated vehicle, the unified kinematic structure model of the articulated vehicle, the reference trajectory centered on the vehicle body, and the vehicle state of the underground loader. The motion execution module is used to execute the coupled control command through the controller and collect the real-time position of the underground loader according to the coupled control command, so as to obtain the new state of the underground loader after motion; The continuous control module is used to replace the vehicle state of the underground loader with the new state after the underground loader moves, and repeat the above steps to realize the continuous control of the underground loader.

Images