Rock drilling rig control method and device
Through Stanley control, MPC hierarchical execution and adaptive weight distribution, the trajectory tracking problem of the drilling rig in complex environments was solved, high-precision and stable trajectory tracking performance was achieved, and the control effect of the drilling rig in complex environments was improved.
Patent Information
- Application Number
- CN202510804236.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2025-06-17
- Publication Date
- 2025-09-09
- Estimated Expiration
- 2045-06-17
AI Technical Summary
The existing control method of the drilling rig cannot take into account both global path optimization and local rapid correction, cannot adapt to sudden changes in the underground environment, and does not deeply couple the non-complete constraint characteristics of the articulated vehicle body, resulting in trajectory tracking accuracy and stability that are difficult to meet the requirements of complex environments.
Stanley control and model predictive control (MPC) are hierarchically executed, combined with a real-time working condition adaptive weight distribution algorithm, and embedded in the dynamic compensation term of the articulation angle. The MPC controller predicts the future state and optimizes the control instructions to achieve accurate and stable trajectory tracking.
It improves the trajectory tracking performance of the drilling rig in complex environments, ensures high precision and stability under various conditions, effectively suppresses vehicle posture oscillation, and improves the real-time and robustness of the control system.
Smart Images
Figure CN120315449B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of electronic digital data processing, and in particular to a rock drilling trolley control method and device. Background Art
[0002] Drilling rigs are heavy machinery used in underground environments such as mines and tunnels. The trajectory tracking performance of their autonomous driving systems directly impacts operational safety and efficiency. Existing trajectory tracking control methods, such as PID and a single trajectory tracking algorithm (Stanley), achieve some degree of trajectory tracking but often fail to meet the high precision and stability requirements in complex terrain and dynamic environments. Traditional PID control relies on linearized model assumptions, making it difficult to address control instability issues caused by strong nonlinearities in underground hydraulic systems and tire slip. While the classic Stanley algorithm can achieve basic path tracking, its fixed look-ahead distance design easily leads to accumulated trajectory cutout errors in narrow curves. Its lack of global optimization capabilities makes it difficult to meet the high-precision requirements of long-distance excavation. While model predictive control (MPC) can improve control quality through multi-objective optimization, its high computational complexity can lead to command delays in scenarios with frequent dynamic obstacles, resulting in delayed obstacle avoidance responses. Furthermore, existing methods generally ignore the kinematic coupling characteristics of the drilling rig's articulated body, which can easily cause body posture oscillation during sharp turns or sudden load changes, threatening operational safety.
[0003] In summary, the inventors discovered that: single control strategies for drilling rigs (such as PID, MPC, or Stanley) cannot balance global path optimization with local rapid deviation correction, resulting in difficulties in balancing accuracy and real-time performance; fixed-parameter control architectures cannot adapt to sudden changes in the underground environment (such as a sharp increase in dust concentration or sudden obstacles), and control quality deteriorates with operating conditions; mainstream algorithms do not deeply couple the nonholonomic constraints of the articulated vehicle body, ignoring the kinematic coupling of the front and rear vehicles when generating steering control variables, exacerbating trajectory tracking deviations; and running multiple controllers in parallel leads to redundant computing resources, which can easily cause system overload on low-computing-power vehicle hardware. Consequently, these strategies cannot meet the trajectory tracking accuracy and stability requirements of drilling rigs in complex environments. Summary of the Invention
[0004] Embodiments of the present invention provide a method and device for controlling a rock drilling rig to achieve more precise and stable control of the movement of the rock drilling rig.
[0005] In a first aspect, an embodiment of the present invention provides a method for controlling a drilling rig, comprising:
[0006] A target motion path of the target drilling rig, a position coordinate of the target drilling rig, and a heading angle of the target drilling rig are obtained.
[0007] Based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig, a lateral error and a heading error are determined.
[0008] A first control component is calculated based on the lateral error and the heading error; and a second control component is calculated based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig.
[0009] Based on the target motion path and the position coordinates of the target drilling rig, the path curvature is obtained; based on the lateral error and the heading error, the error amplitude is calculated; based on the path curvature, the error amplitude and the load, the weight of the first control component and the weight of the second control component are obtained.
[0010] The control amount of the target drilling rig is calculated based on the first control component, the second control component, the weight of the first control component, and the weight of the second control component.
[0011] In one possible implementation, calculating the first control component based on the lateral error and the heading error includes:
[0012] Get the target drilling rig's travel speed.
[0013] Substitute the lateral error, heading error, and driving speed into the Stanley control law to calculate the first control component.
[0014] In one possible implementation, calculating the second control component based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig includes:
[0015] The position coordinates of the target drilling rig and the heading angle of the target drilling rig are used as the current state matrix.
[0016] The second control component is used as the parameter to be determined, and based on the current state matrix and the parameter to be determined, it is input into the prediction model of the MPC controller to obtain the state matrix at the next moment.
[0017] Based on the current state matrix, the state matrix at the next moment and the parameters to be determined, the target control sequence is generated by the MPC controller; wherein the target control sequence is obtained by the MPC controller by minimizing the cost function of the next N steps.
[0018] The steering angular rate constraint, the target drilling rig's articulation angle constraint, the physical limit constraint, and the target control sequence are transformed into a quadratic programming problem.
[0019] The quadratic programming problem is solved by the QP solver using the interior point method or the active set method to obtain the optimal control sequence.
[0020] The first item of the optimal control sequence is taken as the result of the parameter to be determined, that is, the second control component is calculated.
[0021] In a possible implementation, the target motion path includes multiple path points, position coordinates corresponding to each path point, and a tangent direction angle of each path point.
[0022] Based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig, the lateral error and heading error are determined, including:
[0023] Based on the position coordinates of the target drilling rig, find the path point closest to the target drilling rig in the target motion path and record it as the target path point.
[0024] The lateral error is calculated based on the position coordinates of the target path point, the position coordinates of the target drilling rig, and the tangent direction angle of the target path point.
[0025] The difference between the tangent direction angle of the target path point and the heading angle of the target drilling rig is recorded as the heading error.
[0026] In one possible implementation, the path curvature is obtained based on the target motion path and the position coordinates of the target drilling rig, including:
[0027] Based on the target waypoint and the overall movement direction of the target drilling rig, the next waypoint is determined and recorded as the reference waypoint.
[0028] The path curvature is obtained based on the target path point, the reference path point, and the arc length between the target path point and the reference path point.
[0029] In one possible implementation, calculating the error magnitude based on the lateral error and the heading error includes:
[0030] Calculate the square of the lateral error and record it as the first result.
[0031] Calculate the product of the square of the heading error and the normalized weight coefficient, and record it as the second result.
[0032] The square root of the sum of the first result and the second result is taken as the error amplitude.
[0033] In one possible implementation, obtaining a target motion path of a target drilling rig includes:
[0034] Build underground three-dimensional point cloud maps based on lidar, IMU and visual sensors.
[0035] Identify drivable areas and obstacles in underground 3D point cloud maps.
[0036] An initial global path is generated based on the A* algorithm, and the initial global path is optimized to obtain an optimized global path.
[0037] The optimized global path is smoothed using B-spline interpolation to obtain the target motion path.
[0038] In one possible implementation, obtaining the weight of the first control component and the weight of the second control component based on the path curvature, the error amplitude, and the load includes:
[0039] A fuzzy judgment is performed on the path curvature, the error amplitude, and the load according to a preset fuzzy judgment rule to obtain the weight of the first control component and the weight of the second control component.
[0040] In one possible implementation, calculating the control amount of the target drilling rig based on the first control component, the second control component, the weight of the first control component, and the weight of the second control component includes:
[0041] The product of the first control component and the weight of the first control component is calculated and recorded as a third result.
[0042] The product of the second control component and the weight of the second control component is calculated and recorded as a fourth result.
[0043] The sum of the third result and the fourth result is used as the control variable of the target drilling rig.
[0044] In a second aspect, an embodiment of the present invention provides a rock drilling rig control device, comprising:
[0045] The first processing module is used to obtain a target motion path of a target drilling rig, a position coordinate of the target drilling rig, and a heading angle of the target drilling rig.
[0046] The second processing module is used to determine the lateral error and the heading error based on the target motion path, the position coordinates of the target drilling rig and the heading angle of the target drilling rig.
[0047] The third processing module is used to calculate the first control component based on the lateral error and the heading error; and calculate the second control component based on the target motion path, the position coordinates of the target drilling rig and the heading angle of the target drilling rig.
[0048] The fourth processing module is used to obtain the path curvature based on the target motion path and the position coordinates of the target drilling rig; calculate the error amplitude based on the lateral error and the heading error; and obtain the weight of the first control component and the weight of the second control component based on the path curvature, the error amplitude and the load.
[0049] The fifth processing module is configured to calculate a control variable of the target drilling rig based on the first control component, the second control component, the weight of the first control component, and the weight of the second control component.
[0050] In this embodiment of the present invention, the complementary advantages of two control strategies—Stanley control and model predictive control (MPC)—result in more precise control results, effectively improving the trajectory tracking performance of the drilling rig under various environmental conditions. This method not only considers the vehicle's immediate lateral and heading errors but also optimizes control commands by predicting future states through the MPC controller, thereby achieving more accurate and stable trajectory tracking. BRIEF DESCRIPTION OF THE DRAWINGS
[0051] Figure 1 This is a flow chart of an implementation of a drilling rig control method provided by an embodiment of the present invention;
[0052] Figure 2 is a model diagram of a target rock drilling rig provided by an embodiment of the present invention;
[0053] Figure 3 This is a comparison diagram of the figure-8 path tracking provided by an embodiment of the present invention;
[0054] Figure 4a is a graph showing the changing trend of the lateral error and heading error over time provided by an embodiment of the present invention;
[0055] Figure 4b is a lateral error spectrum diagram provided by an embodiment of the present invention;
[0056] Figure 4c is a heading error spectrum diagram provided by an embodiment of the present invention;
[0057] Figure 5 Schematic diagram of a rock drilling rig control device provided in an embodiment of the present invention. DETAILED DESCRIPTION
[0058] Defects of existing technologies: A single control strategy (such as PID, MPC or Stanley) cannot take into account both global path optimization and local rapid correction, resulting in difficulty in coordinating accuracy and real-time performance; the fixed parameter control architecture cannot adapt to sudden changes in the underground environment (such as a sharp increase in dust concentration, sudden obstacles), and the control quality deteriorates with working conditions; the mainstream algorithm does not deeply couple the non-complete constraint characteristics of the articulated body, and ignores the kinematic coupling of the front and rear bodies when generating the steering control quantity, exacerbating trajectory tracking deviations; running multiple controllers in parallel leads to redundant computing resources, which can easily cause system overload in low-computing-power vehicle hardware.
[0059] The improvement direction of the present invention is as follows: MPC global optimization (low-frequency cycle) and Stanley local tracking (high-frequency cycle) are executed in layers to break through the bottleneck of the traditional method in balancing computing resources and control performance; an adaptive weight allocation algorithm based on real-time working conditions (path curvature, tracking error, system load) is used to achieve smooth switching of control modes and conflict resolution; and an articulation angle dynamic compensation term is embedded in the control model to suppress posture oscillations caused by vehicle body coupling motion.
[0060] The embodiments of the present invention will be described in detail below with reference to the accompanying drawings.
[0061] See also Figure 1 , which shows a flow chart of the implementation of the drilling rig control method provided by an embodiment of the present invention, and is described in detail as follows:
[0062] Step 101: Acquire a target motion path of a target drilling rig, a position coordinate of the target drilling rig, and a heading angle of the target drilling rig.
[0063] Exemplarily, the target drilling rig is an articulated vehicle.
[0064] In one possible implementation, obtaining a target motion path of a target drilling rig includes:
[0065] Build underground three-dimensional point cloud maps based on lidar, IMU and visual sensors.
[0066] Identify drivable areas and obstacles in underground 3D point cloud maps.
[0067] An initial global path is generated based on the A* algorithm, and the initial global path is optimized to obtain an optimized global path.
[0068] The optimized global path is smoothed using B-spline interpolation to obtain the target motion path.
[0069] Specifically, the existing technologies such as lidar, IMU and visual sensor fusion technology are first used to identify drivable areas and obstacles, and a three-dimensional point cloud map of the underground environment is constructed in real time. The initial global path is generated based on the A* algorithm and optimized. The optimization goals are the shortest distance, minimum steering energy consumption and avoiding static obstacles. The optimized global path is obtained. Then, the B-spline interpolation algorithm is used to smooth the global path to ensure that the global path is a smooth continuous path to meet the non-holonomic constraints of the articulated vehicle. The dynamic window method DWA adjusts the local trajectory according to the real-time obstacle information. Finally, the target motion path is generated with the current position of the vehicle as the starting point, and the trajectory is parameterized. Each reference path point contains the position coordinates ( ) and the tangent direction angle , for the controller to call.
[0070] Step 102 : determining a lateral error and a heading error based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig.
[0071] In a possible implementation, the target motion path includes multiple path points, position coordinates corresponding to each path point, and a tangent direction angle of each path point.
[0072] Determining the lateral error and the heading error based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig may include:
[0073] Based on the position coordinates of the target drilling rig, find the path point closest to the target drilling rig in the target motion path and record it as the target path point.
[0074] The lateral error is calculated based on the position coordinates of the target path point, the position coordinates of the target drilling rig, and the tangent direction angle of the target path point.
[0075] The difference between the tangent direction angle of the target path point and the heading angle of the target drilling rig is recorded as the heading error.
[0076] Specifically, first find the path point closest to the vehicle's current position in the reference path , commonly used methods include Euclidean distance method: (The coordinates of the drilling rig are: And the heading angle of the drilling rig: , the location coordinates of the path points: And the tangent direction of the path point: ) and an optimization method that uses KD-Tree to accelerate search and reduce computational complexity.
[0077] To calculate the lateral error, the vehicle coordinates need to be projected onto the path points In the tangent coordinate system, calculate the vertical deviation:
[0078] 1. Construct a local coordinate system: The origin is the path tangent direction is the X axis, and the normal direction is the Y axis.
[0079] 2. Position the vehicle Transform to the local coordinate system:
[0080]
[0081] in, To obtain the lateral error.
[0082] Then calculate the heading error: the heading error represents the deviation between the vehicle's current heading angle and the tangent direction of the reference path. The calculation formula is: , The nearest path point The tangent direction angle, is the current heading angle of the vehicle.
[0083] Step 103 : Calculate a first control component based on the lateral error and the heading error; and calculate a second control component based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig.
[0084] For example, to better understand the overall motion of the entire target drilling rig, Figure 2 As shown in the figure, the model diagram of the target drilling rig is given. P 1 represents the midpoint of the front half of the target drilling rig, P 2 represents the midpoint of the rear half of the target drilling rig.
[0085] exist H The point (the hinge point between the front and rear halves of the target drilling rig) can be written as:
[0086] (1)
[0087] For a right triangle and , we can get:
[0088] (2)
[0089] (3)
[0090] Substituting into (1) we can get:
[0091] (4)
[0092] Then, according to the right triangle , which can be written as the following formula:
[0093] (5)
[0094] (6)
[0095] Simplified turning radius:
[0096] (7)
[0097] (8)
[0098] The two parts of the car body run along circles of different radii, and the ratio of the radii of curvature is:
[0099] (9)
[0100] Only when It is always equal to 1.
[0101] As a result of this difference in radius, the ground speed of the two halves of the vehicle is also different when turning. The vehicle has a separate differential for the front and rear wheels, each driven by the output of the transmission. The input speed of the differential is the average speed of its two associated wheels.
[0102] (10)
[0103] (11)
[0104] in, is the wheel spacing along the axle, represents the average speed of the two relevant wheels of the front half of the vehicle, represents the average speed of the two relevant wheels at the rear half of the vehicle, is the angular velocity of the vehicle moving along the arc. The ratio of the average speeds of the front and rear wheels can be expressed as formula (12):
[0105] (12)
[0106] Only in linear motion, , or the case of turning For articulated vehicles, this effect is small at non-zero steering angles, but the rear wheels need to be effectively 2% faster than the drive speed, and they will be dragged by the front wheels, causing tire wear and increased stress on the joints.
[0107] According to the attached Figure 2 , for the front half of the vehicle:
[0108] (13)
[0109] (14)
[0110] in, v is the velocity of the center of mass of the front half of the vehicle, express The guide, express The guide. and The relationship is given by the following formula:
[0111] (15)
[0112] (16)
[0113] Known:
[0114] (17)
[0115] A non-sliding rolling constraint (non-holonomic constraint) means that there can be no motion parallel to the axis, that is:
[0116] (18)
[0117] (19)
[0118] Differentiate (4) and (5) in time, substitute them into (2), (3), (7) and (8), and then simplify to obtain:
[0119] (20)
[0120] That is, the rate of change of the heading angle of the front half of the vehicle and the center of mass velocity of the front half of the vehicle v With articulation angle and hinge angle change rate The relationship is a dynamic coupling.
[0121] In one possible implementation, calculating the first control component based on the lateral error and the heading error may include:
[0122] Get the target drilling rig's travel speed.
[0123] Substitute the lateral error, heading error, and driving speed into the Stanley control law to calculate the first control component.
[0124] Specifically, in Stanley (trajectory tracking algorithm), the Stanley control law is as follows:
[0125]
[0126] in, It represents the heading error. represents the lateral error, The target drilling rig speed is represented by the Stanley control law, which uses the nonlinear feedback term Generate steering angle increments to make the vehicle converge to the path; directly superimpose the heading error , ensuring that the vehicle's heading is consistent with the path tangent. Because the Stanley control law takes into account the nonholonomic constraints of the articulated body, it can be used to compensate for the kinematic coupling effects of the articulated body, improve local tracking accuracy, and suppress heading oscillations caused by body coupling.
[0127] In one possible implementation, calculating the second control component based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig may include:
[0128] The position coordinates of the target drilling rig and the heading angle of the target drilling rig are used as the current state matrix.
[0129] The second control component is used as the parameter to be determined, and based on the current state matrix and the parameter to be determined, it is input into the prediction model of the MPC controller to obtain the state matrix at the next moment.
[0130] Based on the current state matrix, the state matrix at the next moment and the parameters to be determined, the target control sequence is generated by the MPC controller; wherein the target control sequence is obtained by the MPC controller by minimizing the cost function of the next N steps.
[0131] The steering angular rate constraint, the target drilling rig's articulation angle constraint, the physical limit constraint, and the target control sequence are transformed into a quadratic programming problem.
[0132] The quadratic programming problem is solved by the QP solver using the interior point method or the active set method to obtain the optimal control sequence.
[0133] The first item of the optimal control sequence is taken as the result of the parameter to be determined, that is, the second control component is calculated.
[0134] Exemplarily, the prediction model of the MPC controller is:
[0135]
[0136] in, The wheelbase of the target drilling rig is is the distance from the front axle to the center of mass of the target drilling rig, 、 、 Separate moments k The position coordinates and heading angle of the target drilling rig at time is the target drilling rig travel speed, For the moment k The target drilling rig steering angle at is the discrete time step of MPC, and the steering angular rate constraint is: Among them, the update term of the heading angle in the prediction model is It includes the constraint relationship of angular velocity, which is formula (20).
[0137] Assume that the current state matrix is , the control input is , so the prediction model is abbreviated as: , and get the state matrix at the next moment.
[0138] The MPC controller generates the target control sequence by minimizing the cost function for the next N steps:
[0139]
[0140] in, is the tracking error term, and the weight matrix is ,in, Represent the penalty position coordinates and the penalty heading deviation respectively; is the control input, the weight coefficient R , suppress the steering angle amplitude; is the terminal cost term, weight coefficient P , ensuring the stability of the prediction terminal.
[0141] The steering angle rate constraint is given: , that is , ( i =1,2,..., N ); In the hinge angle constraint, the hinge angle change rate Dynamically coupled with the heading angle, the articulation angle constraint can be used as a nonlinear constraint, and the target control sequence can be converted into an optimization problem. The physical limit constraint is specified as: .
[0142] Then the optimization problem of the above-mentioned numerous constraints and target control sequences is transformed into a quadratic programming (QP) problem to obtain:
[0143]
[0144] The optimization variables are U , , is the steering angle sequence for the next N steps; Hessian matrix H With the gradient vector c The weight coefficients in the target control sequence Q , P , R generate.
[0145] Finally, the optimization problem is solved by the QP solver using the interior point method or the effective set method to obtain the optimal control sequence. , take the first item of the sequence as the current steering angle output: .
[0146] Exemplarily, MPC is a low-frequency layer with an execution cycle of 100-200ms. It updates the global optimization instructions once every certain time window. The input is the global reference trajectory (target motion path), real-time vehicle posture (position coordinates of the target drilling rig and the heading angle of the target drilling rig), and sensor data. The output is the second control component; Stanley is a high-frequency layer with an execution cycle of 10-50ms. It responds to vehicle posture deviation in real time. The input is real-time error (lateral error, heading error) and vehicle speed (driving speed), and the output is the first control component.
[0147] Step 104: Obtain a path curvature based on the target motion path and the position coordinates of the target drilling rig; calculate an error amplitude based on the lateral error and the heading error; and obtain a weight of the first control component and a weight of the second control component based on the path curvature, the error amplitude, and the load.
[0148] In one possible implementation, obtaining the path curvature based on the target motion path and the position coordinates of the target drilling rig may include:
[0149] Based on the target waypoint and the overall movement direction of the target drilling rig, the next waypoint is determined and recorded as the reference waypoint.
[0150] The path curvature is obtained based on the target path point, the reference path point, and the arc length between the target path point and the reference path point.
[0151] For example, path curvature k The calculation of can be calculated by the geometric characteristics of the reference trajectory:
[0152]
[0153] in, is the arc length between adjacent reference path points, It is The tangent direction angle of each path point.
[0154] Alternatively, it can be calculated using the tangent vector at the path point:
[0155]
[0156] in, For waypoints i The tangent vector of .
[0157] In one possible implementation, calculating the error magnitude based on the lateral error and the heading error may include:
[0158] Calculate the square of the lateral error and record it as the first result.
[0159] Calculate the product of the square of the heading error and the normalized weight coefficient, and record it as the second result.
[0160] The square root of the sum of the first result and the second result is taken as the error amplitude.
[0161] For example, the error magnitude e The calculation of the lateral error Heading error calculate:
[0162]
[0163] in, is the normalized weight coefficient, usually set to 0.1 to balance the unit difference, is the heading error, is the heading error.
[0164] The two (MPC control and Stanley control) work together to achieve collaborative control through dynamic weights:
[0165] In dynamic weight allocation, when the path curvature is high (in a sharp bend) or the tracking error exceeds the threshold, the weight of the second control component increases and the weight of the first control component decreases to avoid local control failure; on a straight path or when the error is small, the weight of the first control component increases to give full play to its fast response advantage.
[0166] In one possible implementation, obtaining the weight of the first control component and the weight of the second control component based on the path curvature, the error amplitude, and the load includes:
[0167] A fuzzy judgment is performed on the path curvature, the error amplitude, and the load according to a preset fuzzy judgment rule to obtain the weight of the first control component and the weight of the second control component.
[0168] For example, the path curvature ranges from 0 to 0.5 m. -1 , the fuzzy judgment result of the path curvature is:
[0169] When the path curvature is in the range of [0-0.15), the fuzzy judgment result of the path curvature is low.
[0170] When the path curvature is in the range of [0.15-0.3), the fuzzy judgment result of the path curvature is medium.
[0171] When the path curvature is in the range of [0.3-0.5], the fuzzy judgment result of the path curvature is high.
[0172] For example, the error amplitude range is 0-0.3m, and the fuzzy judgment result of the error amplitude is:
[0173] When the error amplitude is in the range of [0-0.1), the fuzzy judgment result of the error amplitude is small.
[0174] When the error amplitude is in the range of [0.1-0.15), the fuzzy judgment result of the error amplitude is medium.
[0175] When the error amplitude is in the range of [0.15-0.3], the fuzzy judgment result of the error amplitude is large.
[0176] For example, the load range is 0%-100%, where the load specifically refers to the hydraulic pressure of the target drilling rig. The fuzzy judgment result of the load is:
[0177] When the load is within the range of [0-30%), the fuzzy load judgment result is light. A typical scenario is generally driving at a constant speed on a flat road.
[0178] When the load is within the range of [30%-60%), the fuzzy load judgment result is medium. Typical scenarios are generally gentle slopes and small-angle curves.
[0179] When the load is within the range of [60%-100%], the fuzzy load judgment result is heavy. Typical scenarios are steep slope climbing, rough terrain, or sharp turns to avoid obstacles.
[0180] For example, the preset fuzzy judgment rule may be:
[0181] The weight of the second control component is determined based on the fuzzy judgment results of the path curvature, error amplitude and load; wherein, since the sum of the weight of the second control component and the weight of the first control component is 1, then after determining the weight of the second control component, the weight of the first control component can be obtained.
[0182] The specific logic for determining the weight of the second control component is as follows: High path curvature forces the second control component to increase its weight to ensure MPC's global path tracking capability. When the error exceeds a threshold (0.15m), the second control component's weight is increased to ensure localized loss of control is suppressed. Under high load, the second control component's weight is reduced to prevent MPC calculation delays from causing command lag.
[0183] Specifically, all the weights of the second control component are shown in Table 1:
[0184] Table 1 All values of the weight of the second control component
[0185]
[0186]
[0187] Step 105 : Calculate the control amount of the target drilling rig based on the first control component, the second control component, the weight of the first control component, and the weight of the second control component.
[0188] In one possible implementation, step 105 may include:
[0189] The product of the first control component and the weight of the first control component is calculated and recorded as a third result.
[0190] The product of the second control component and the weight of the second control component is calculated and recorded as a fourth result.
[0191] The sum of the third result and the fourth result is used as the control variable of the target drilling rig.
[0192] This drilling rig control method leverages the complementary strengths of two control strategies—Stanley control and model predictive control (MPC)—to achieve more precise control results and effectively improve the rig's trajectory tracking performance under various environmental conditions. This method not only considers the vehicle's immediate lateral and heading errors but also optimizes control commands by predicting future states using the MPC controller, thereby achieving more accurate and stable trajectory tracking.
[0193] like Figure 3 As shown in the figure, the actual motion path of the vehicle when it is controlled is compared with the target motion path (reference path). It can be seen that the motion path of the vehicle when it is controlled is roughly the same as the target motion path. When serious deviations occur, timely adjustments can be made, thereby completing the entire process.
[0194] like Figure 4a 、 Figure 4b and Figure 4c As shown in the figure, the lateral error shows a convergence trend in the time domain, and the lateral error spectrum can reveal the key characteristics of the control system that cannot be observed directly. The lateral error spectrum obtained by FFT transformation ( Figure 4b ) can identify the dominant frequency components in the lateral error and clarify the frequency range that the control algorithm needs to optimize. The heading error gradually stabilizes in the time domain, and the heading error spectrum ( Figure 4c ) can analyze problems in dynamic response. For example, the low-frequency dominant peak reflects the inertial delay of heading control, and the MPC prediction time domain needs to be optimized; the heading error spectrum can also quantify the frequency domain impact of sudden interference and guide the design of anti-interference algorithms.
[0195] In addition, in Figure 4, interference events are added at times t1 and t2, respectively. Although the addition of interference causes the heading error and lateral error to increase sharply, they can still be quickly adjusted within a very short time (5 seconds) and quickly restored to a stable range, further illustrating the efficiency of the present application solution.
[0196] In terms of stability, it effectively balances the conflict between global planning and local correction, evaluates key parameters such as path curvature, tracking error and system load in real time through the fuzzy rule base, dynamically adjusts the weight ratio of model predictive control (MPC) and Stanley algorithm, and suppresses the oscillation risk caused by sudden changes in control mode in traditional methods.
[0197] In terms of rapid response, a spatiotemporal decoupling strategy is adopted: the MPC layer performs global trajectory optimization with a low-frequency cycle, and the Stanley layer implements real-time correction with a millisecond-level high-frequency cycle. Combined with a dynamic switching mechanism, it seamlessly enters the rapid response mode when sudden obstacles occur or computing resources are limited, ensuring the timeliness of control in complex environments.
[0198] In terms of tracking accuracy, the robustness of posture perception is improved through multi-sensor fusion, and the dynamic forward distance adjustment and articulation compensation terms of the Stanley algorithm are used to eliminate the kinematic model deviation. At the same time, the MPC layer embeds the geometric constraints of the tunnel and the dynamic characteristics of the equipment to achieve optimal approximation control of the global path.
[0199] This method targets the control requirements of drilling rigs under complex working conditions. Through layered collaboration and dynamic adaptation mechanisms, it significantly improves the control reliability and trajectory tracking accuracy under harsh working conditions such as narrow tunnels and slippery roads. It has high real-time performance and high control accuracy.
[0200] It should be understood that the size of the serial numbers of the steps in the above embodiments 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 the present invention.
[0201] The following are device embodiments of the present invention. For details not fully described therein, reference may be made to the corresponding method embodiments described above.
[0202] Figure 5 A schematic structural diagram of a rock drilling rig control device according to an embodiment of the present invention is shown. For ease of explanation, only the parts related to the embodiment of the present invention are shown, which are described in detail as follows:
[0203] like Figure 5 As shown, the drilling rig control system includes:
[0204] The first processing module 201 is configured to obtain a target motion path of a target drilling rig, a position coordinate of the target drilling rig, and a heading angle of the target drilling rig.
[0205] The second processing module 202 is configured to determine a lateral error and a heading error based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig.
[0206] The third processing module 203 is configured to calculate a first control component based on the lateral error and the heading error; and calculate a second control component based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig.
[0207] The fourth processing module 204 is used to obtain the path curvature based on the target motion path and the position coordinates of the target drilling rig; calculate the error amplitude based on the lateral error and the heading error; and obtain the weight of the first control component and the weight of the second control component based on the path curvature, the error amplitude and the load.
[0208] The fifth processing module 205 is configured to calculate the control amount of the target drilling rig based on the first control component, the second control component, the weight of the first control component, and the weight of the second control component.
[0209] In a possible implementation, the third processing module 203 may be configured to:
[0210] Get the target drilling rig's travel speed.
[0211] Substitute the lateral error, heading error, and driving speed into the Stanley control law to calculate the first control component.
[0212] In a possible implementation, the third processing module 203 may:
[0213] The position coordinates of the target drilling rig and the heading angle of the target drilling rig are used as the current state matrix.
[0214] The second control component is used as the parameter to be determined, and based on the current state matrix and the parameter to be determined, it is input into the prediction model of the MPC controller to obtain the state matrix at the next moment.
[0215] Based on the current state matrix, the state matrix at the next moment and the parameters to be determined, the target control sequence is generated by the MPC controller; wherein the target control sequence is obtained by the MPC controller by minimizing the cost function of the next N steps.
[0216] The steering angular rate constraint, the target drilling rig's articulation angle constraint, the physical limit constraint, and the target control sequence are transformed into a quadratic programming problem.
[0217] The quadratic programming problem is solved by the QP solver using the interior point method or the active set method to obtain the optimal control sequence.
[0218] The first item of the optimal control sequence is taken as the result of the parameter to be determined, that is, the second control component is calculated.
[0219] In a possible implementation, the target motion path includes multiple path points, position coordinates corresponding to each path point, and a tangent direction angle of each path point.
[0220] The second processing module 202 can be used to:
[0221] Based on the position coordinates of the target drilling rig, find the path point closest to the target drilling rig in the target motion path and record it as the target path point.
[0222] The lateral error is calculated based on the position coordinates of the target path point, the position coordinates of the target drilling rig, and the tangent direction angle of the target path point.
[0223] The difference between the tangent direction angle of the target path point and the heading angle of the target drilling rig is recorded as the heading error.
[0224] In a possible implementation, the fourth processing module 204 may be configured to:
[0225] Based on the target waypoint and the overall movement direction of the target drilling rig, the next waypoint is determined and recorded as the reference waypoint.
[0226] The path curvature is obtained based on the target path point, the reference path point, and the arc length between the target path point and the reference path point.
[0227] In a possible implementation, the fourth processing module 204 may be configured to:
[0228] Calculate the square of the lateral error and record it as the first result.
[0229] Calculate the product of the square of the heading error and the normalized weight coefficient, and record it as the second result.
[0230] The square root of the sum of the first result and the second result is taken as the error amplitude.
[0231] In a possible implementation, the first processing module 201 may be configured to:
[0232] Build underground three-dimensional point cloud maps based on lidar, IMU and visual sensors.
[0233] Identify drivable areas and obstacles in underground 3D point cloud maps.
[0234] An initial global path is generated based on the A* algorithm, and the initial global path is optimized to obtain an optimized global path.
[0235] The optimized global path is smoothed using B-spline interpolation to obtain the target motion path.
[0236] In a possible implementation, the fourth processing module 204 may be configured to:
[0237] A fuzzy judgment is performed on the path curvature, the error amplitude, and the load according to a preset fuzzy judgment rule to obtain the weight of the first control component and the weight of the second control component.
[0238] In a possible implementation, the fifth processing module 205 may be configured to:
[0239] The product of the first control component and the weight of the first control component is calculated and recorded as a third result.
[0240] The product of the second control component and the weight of the second control component is calculated and recorded as a fourth result.
[0241] The sum of the third result and the fourth result is used as the control variable of the target drilling rig.
[0242] The embodiments described above are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention, and should all be included in the scope of protection of the present invention.
Claims
1. A drilling rig control method, characterized in that: include: Obtaining a target motion path of a target drilling rig, a position coordinate of the target drilling rig, and a heading angle of the target drilling rig; Determining a lateral error and a heading error based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig; calculating a first control component based on the lateral error and the heading error; Calculating a second control component based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig; Obtaining a path curvature based on the target motion path and the position coordinates of the target drilling rig; calculating an error magnitude based on the lateral error and the heading error; Obtaining a weight of a first control component and a weight of a second control component based on the path curvature, the error amplitude, and the load; Calculating a control variable of a target drilling rig based on the first control component, the second control component, a weight of the first control component, and a weight of the second control component; The obtaining of the weight of the first control component and the weight of the second control component based on the path curvature, the error amplitude, and the load includes: Performing fuzzy judgment on the path curvature, the error amplitude, and the load according to a preset fuzzy judgment rule to obtain a weight of the first control component and a weight of the second control component; When the path curvature is in the range of [0-0.15), the fuzzy judgment result of the path curvature is low; When the path curvature is in the range of [0.15-0.3), the fuzzy judgment result of the path curvature is medium; When the path curvature is in the range of [0.3-0.5], the fuzzy judgment result of the path curvature is high; When the error amplitude is within the range of [0-0.1), the fuzzy judgment result of the error amplitude is small; When the error amplitude is in the range of [0.1-0.15), the fuzzy judgment result of the error amplitude is medium; When the error amplitude is in the range of [0.15-0.3], the fuzzy judgment result of the error amplitude is large; When the load is in the range of [0-30%), the fuzzy judgment result of the load is light; When the load is in the range of [30%-60%), the fuzzy judgment result of the load is medium; When the load is within the range of [60%-100%], the fuzzy judgment result of the load is heavy; The preset fuzzy judgment rules are: The weight of the second control component is determined according to the fuzzy judgment results of the path curvature, the error amplitude and the load; wherein, the sum of the weight of the second control component and the weight of the first control component is 1.
2. The drilling rig control method according to claim 1, wherein: The calculating a first control component based on the lateral error and the heading error comprises: Obtaining the travel speed of the target drilling rig; The lateral error, the heading error, and the driving speed are substituted into the Stanley control law to calculate the first control component.
3. The drilling rig control method according to claim 1, wherein: The calculating the second control component based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig comprises: Taking the position coordinates of the target drilling rig and the heading angle of the target drilling rig as the current state matrix; Taking the second control component as the parameter to be determined, based on the current state matrix and the parameter to be determined, inputting them into the prediction model of the MPC controller to obtain the state matrix at the next moment; Based on the current state matrix, the state matrix at the next moment, and the desired parameter, a target control sequence is generated by the MPC controller; wherein the target control sequence is obtained by the MPC controller by minimizing a cost function for the next N steps; Transforming the steering angular rate constraint, the target drilling rig's articulation angle constraint, the physical limit constraint, and the target control sequence into a quadratic programming problem; Solve the quadratic programming problem using the interior point method or the active set method through a QP solver to obtain an optimal control sequence; The first item of the optimal control sequence is used as the result of the parameter to be determined, that is, the second control component is calculated.
4. The drilling rig control method according to claim 1, wherein: The target motion path includes multiple path points, the position coordinates corresponding to each path point, and the tangent direction angle of each path point; The determining of the lateral error and the heading error based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig comprises: Based on the position coordinates of the target drilling rig, searching for a path point closest to the target drilling rig in the target motion path, and recording the path point as a target path point; Calculating the lateral error based on the position coordinates of the target path point, the position coordinates of the target drilling rig, and the tangent direction angle of the target path point; The difference between the tangent direction angle of the target path point and the heading angle of the target drilling rig is recorded as the heading error.
5. The drilling rig control method according to claim 4, wherein: The obtaining of the path curvature based on the target motion path and the position coordinates of the target drilling rig includes: Determining a next path point based on the target path point and the overall movement direction of the target drilling rig, and recording it as a reference path point; A path curvature is obtained based on the target path point, the reference path point, and the arc length between the target path point and the reference path point.
6. The drilling rig control method according to claim 1, wherein: The calculating the error amplitude based on the lateral error and the heading error includes: Calculate the square of the lateral error and record it as a first result; Calculate the product of the square of the heading error and the normalized weight coefficient, and record it as a second result; The square root of the sum of the first result and the second result is used as the error amplitude.
7. The drilling rig control method according to claim 1, wherein: The obtaining of the target motion path of the target drilling rig includes: Build underground 3D point cloud maps based on lidar, IMU and visual sensors; Identifying drivable areas and obstacles in the underground three-dimensional point cloud map; Generate an initial global path based on the A* algorithm, and optimize the initial global path to obtain an optimized global path; The optimized global path is smoothed by using B-spline interpolation to obtain the target motion path.
8. The drilling rig control method according to claim 1, wherein: The calculating the control amount of the target drilling rig based on the first control component, the second control component, the weight of the first control component, and the weight of the second control component includes: Calculating a product of the first control component and the weight of the first control component, and recording the product as a third result; Calculate the product of the second control component and the weight of the second control component, and record it as a fourth result; The sum of the third result and the fourth result is used as the control variable of the target drilling rig.
9. A rock drilling rig control device, characterized in that: include: A first processing module is configured to obtain a target motion path of a target drilling rig, a position coordinate of the target drilling rig, and a heading angle of the target drilling rig; a second processing module for determining a lateral error and a heading error based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig; a third processing module, configured to calculate a first control component based on the lateral error and the heading error; Calculating a second control component based on the target motion path, the position coordinates of the target drilling rig, and the heading angle of the target drilling rig; a fourth processing module, configured to obtain a path curvature based on the target motion path and the position coordinates of the target drilling rig; calculating an error magnitude based on the lateral error and the heading error; Obtaining a weight of a first control component and a weight of a second control component based on the path curvature, the error amplitude, and the load; a fifth processing module, configured to calculate a control variable of a target drilling rig based on the first control component, the second control component, a weight of the first control component, and a weight of the second control component; The fourth processing module is configured to: Performing fuzzy judgment on the path curvature, the error amplitude, and the load according to a preset fuzzy judgment rule to obtain a weight of the first control component and a weight of the second control component; When the path curvature is in the range of [0-0.15), the fuzzy judgment result of the path curvature is low; When the path curvature is in the range of [0.15-0.3), the fuzzy judgment result of the path curvature is medium; When the path curvature is in the range of [0.3-0.5], the fuzzy judgment result of the path curvature is high; When the error amplitude is within the range of [0-0.1), the fuzzy judgment result of the error amplitude is small; When the error amplitude is in the range of [0.1-0.15), the fuzzy judgment result of the error amplitude is medium; When the error amplitude is in the range of [0.15-0.3], the fuzzy judgment result of the error amplitude is large; When the load is in the range of [0-30%), the fuzzy judgment result of the load is light; When the load is in the range of [30%-60%), the fuzzy judgment result of the load is medium; When the load is within the range of [60%-100%], the fuzzy judgment result of the load is heavy; The preset fuzzy judgment rules are: The weight of the second control component is determined according to the fuzzy judgment results of the path curvature, the error amplitude and the load; wherein, the sum of the weight of the second control component and the weight of the first control component is 1.