An underground engineering operation trolley mechanical arm automatic control method and system based on a hybrid optimization strategy

Abstract

The present application relates to a kind of underground engineering operation trolley mechanical arm automatic control method and system based on hybrid optimization strategy, the method includes: the coordinate system conversion parameter of conversion to the coordinate system set with the starting pose of mechanical arm is calculated;The coordinates of target point are converted into the coordinate system of mechanical arm based on the coordinate system conversion parameter, the target pose of mechanical arm is determined based on the coordinates of target point;Search interval of joint state vector is constructed, target function and gradient update function of joint movement are constructed, and convergence solution is obtained as the initial solution of joint state vector by iterative calculation;Local discrete search is carried out to the vicinity of initial solution, and the optimal solution of joint state vector is obtained as target joint state;Starting joint state and target joint state are used as the boundary point of graph search, tree structure is constructed in joint space by RRT sampling, and joint trajectory is generated by combining target bias sampling and rewiring optimization;Significantly improve the operation success rate, precision and reliability of mechanical arm in underground working condition.

Description

Technical Field

[0001] This invention relates to the field of underground engineering operations, and in particular to an automatic control method for a robotic arm of an underground engineering operation trolley based on a hybrid optimization strategy. Background Technology

[0002] Underground engineering work trolleys, such as mining trolleys, are key engineering equipment mainly used in mining and underground engineering scenarios. The main rock drilling device consists of three robotic arms and one telescopic linkage. The purpose is to use the robotic arms to move to a fixed position, and then perform the corresponding drilling operation using a hydraulic rock drill. For this type of machine, existing technology mainly relies on hand-eye calibration, which, simply put, solves the problem of coordinate communication between the robotic arm and the camera. The camera can recognize the position of an object in its own coordinate system, but the robotic arm only understands its own base coordinate system / endpoint coordinate system. It does not know the position of the object in its own coordinate system and cannot accurately reach out to pick it up.

[0003] Inverse kinematics of a robotic arm solves for the known target position (position and orientation) of the end effector and then deduces all joint angles / displacements that satisfy that orientation. This is a crucial step connecting the "target position" and "joint execution." Commonly used methods include analytical methods, numerical methods, and their optimization algorithms.

[0004] Due to the dim lighting conditions underground, it's impossible to confirm the camera's stable operation. Furthermore, there are unavoidable problems regardless of whether the camera is mounted near the drilling location or at the end of the robotic arm. For example, when there are many holes spaced far apart, a single camera mounted near the first drilling point cannot calibrate all points, while a camera mounted at the end of the robotic arm is extremely susceptible to dirt. Therefore, the traditional hand-eye calibration method is not feasible for the current project.

[0005] Existing methods for inverse computation of robotic arms all have limitations. Analytical methods are only suitable for robotic arms with simple structures and few degrees of freedom. For mining trolleys, it is impossible to derive a general analytical formula, thus rendering them almost unusable commercially. Numerical iterative methods heavily rely on the choice of initial values; if the initial values ​​are too far from the true solution, the iteration may not converge. Optimization algorithms are prone to getting trapped in local optima, and the difficulty in adjusting parameters such as the learning rate and number of iterations can lead to slow convergence or divergence. Grid search methods are computationally intensive and extremely resource-intensive. Summary of the Invention

[0006] This invention addresses the technical problems existing in the prior art by providing an automatic control method and system for a robotic arm of an underground engineering operation trolley based on a hybrid optimization strategy. By forming a synergy among multiple levels such as a unified coordinate system, hybrid optimization inverse solution, and optimized path planning, the success rate, accuracy, and reliability of the robotic arm in complex working conditions such as underground engineering are significantly improved.

[0007] According to a first aspect of the present invention, an automatic control method for a robotic arm of an underground engineering work trolley based on a hybrid optimization strategy is provided, comprising: Step 1: Calculate the coordinate system transformation parameters to the coordinate system set with the robot arm's initial pose; based on the coordinate system transformation parameters, transform the coordinates of the target point to the robot arm's coordinate system, and determine the target pose of the robot arm based on the coordinates of the target point; Step 2: Define a joint state vector representing the joint movement state; construct the search interval of the joint state vector, construct the objective function and gradient update function for joint movement, and perform iterative calculation to obtain the convergent solution as the initial solution of the joint state vector; perform a local discrete search in the vicinity of the initial solution to obtain the optimal solution of the joint state vector as the target joint state. Step 3: Using the initial joint state and the target joint state as boundary points for graph search, a tree structure is constructed in the joint space through RRT sampling. Combined with target bias sampling and rewire optimization, the joint trajectory is generated.

[0008] Based on the above technical solution, the present invention can also be improved as follows.

[0009] Optionally, the process of calculating the coordinate system transformation parameters to the coordinate system set with the robot arm's initial pose in step 1 includes: Step 101: Set an initial coordinate system field according to the site environment, and select three non-collinear points under the coordinate system field; Step 102: The coordinates of the three points in the mapping coordinate system map are obtained by using the lidar mounted on the underground engineering work vehicle. Step 103: Measure the change in position of the lidar from the starting pose of the robotic arm, and calculate the coordinate system transformation parameters from the coordinate system field to the coordinate system base_link of the robotic arm based on the change.

[0010] Optionally, the joint state vector in step 2 includes joint position. and joint physical boundaries; The joint position , This refers to the angle of the rotating joint; The displacement of the locating / telescalating joint; The physical boundaries of the joint include the upper limit of the joint position. and lower limit .

[0011] Optionally, the process of calculating the initial solution of the joint state vector goal_joint_state in step 2 includes: Step 201, using the initial value of the joint state vector. Centered on the search interval, the search range is constructed as follows: ; in, These are parameters set according to joint type and mechanism redundancy. Step 202: Perform a coarse mesh search in the search interval with a step size s_c, calculate the end pose error and constraint penalty for each mesh point, and obtain the candidate set and the current optimal solution; Step 203: Using the optimal solution obtained from the coarse grid search as the initial value, construct the objective function as follows: ; in, , , and These are the weighting coefficients. and The end position / attitude obtained from the forward solution, and The end position / attitude of the optimal solution obtained from the coarse-grid search. Penalties are used to indicate joint limitation / collision / singularity. For joint change smoothing; Step 204, set the gradient update function as follows: ; The set update step size; The gradient vector; and The kth and k+1th generations respectively ; With the objective function as the goal, update based on the gradient update function. The update stops when the pose error is below a threshold, the objective function decreases insufficiently, or the number of iterations reaches the upper limit, thus obtaining the initial solution. .

[0012] Optionally, the process of obtaining the target joint state goal_joint_state in step 2 includes: Step 205, construct the initial solution The surrounding area is: ;in, ; Step 206: Perform a fine mesh search in the nearby region based on step size s_f, where s_f ≪ s_c; calculate the end pose error and constraint penalty for each mesh point, and output the target joint state goal_joint_state.

[0013] Optionally, step 3 includes: Step 301: Using the starting joint state `start_joint_state` and the target joint state `goal_joint_state` as boundary conditions, obstacle information `obstacles` is introduced, and a feasible joint path `joint_path` is generated using RRT. The feasible joint path `joint_path` is then persisted as a usable path file. The feasible joint path `joint_path` includes: a discrete waypoint sequence { }, Time parameters { } / Sampling period and constraints { }; Step 302: The controller reads the available path file and interpolates the discrete waypoint sequence {} into a reference trajectory. ; Step 303, the calculation error is: ; The actual trajectory is determined based on the current pose (current_pose), feedback from each joint encoder, and the actual travel length (extension_length) of the extension joint. Step 304: The controller outputs control quantity for each joint using a feedforward + PID closed-loop strategy based on the error; when a limit switch, abnormal load, or tracking error exceeding the threshold is detected, the controller enters a degraded or safe shutdown process.

[0014] Optionally, step 301 further includes: using rewire and Bezier smoothing to make the feasible joint path joint_path meet the joint velocity / acceleration limit and continuity requirements; The available path file is an Excel file.

[0015] Optionally, in step 304, the control quantity includes: joint drive command control_value, joint enable / stop status joint_enable_status, joint forward and reverse direction joint_direction, and extension joint target stroke extension_length_command.

[0016] Optionally, after step 3, the method further includes: combining multi-sensor information with host computer control commands to drive the actuator to perform operations; The drive actuator includes: a lever arm, a pusher, a retainer, and a rotating rod; The multi-sensor information includes: joint encoder angle, extension stroke, limit switch status, attitude / tilt angle, and environmental / obstacle detection results; The host computer commands include: target hole position / target pose, operating mode, target depth setting, allowable error threshold and safety boundary; the operating modes include: positioning, drilling, rod retraction, pause and emergency stop; The controller calculates the target parameters for the current stage based on the multi-sensor information and the host computer control commands, and sends them to the lower-level drive actuators in state machine order; the target parameters include: target pose, propulsion amount, and speed / force control setpoint.

[0017] According to a second aspect of the present invention, an automatic control system for a robotic arm of an underground engineering work trolley based on a hybrid optimization strategy is provided, comprising: a perception and coordinate transformation layer, a motion planning layer, an execution control layer, and a task execution layer; The perception and coordinate transformation layer is used to calculate coordinate system transformation parameters to transform to a coordinate system set with the robot arm's initial pose; based on the coordinate system transformation parameters, the coordinates of the target point are transformed to the robot arm's coordinate system, and the target pose of the robot arm is determined based on the coordinates of the target point; The motion planning layer is used to define a joint state vector representing the joint movement state; construct a search interval for the joint state vector; construct an objective function and gradient update function for joint movement; perform iterative calculations to obtain a converged solution as the initial solution of the joint state vector; and perform a local discrete search in the vicinity of the initial solution to obtain the optimal solution of the joint state vector as the target joint state. The execution control layer is used to take the initial joint state and the target joint state as boundary points of graph search, construct a tree structure in the joint space through RRT sampling, and generate joint trajectories by combining target bias sampling and rewire optimization. The task execution layer is used to combine multi-sensor information with host computer control commands to drive the execution mechanism to perform operations.

[0018] This invention provides an automatic control method and system for an underground engineering work trolley robotic arm based on a hybrid optimization strategy. By organically combining non-camera multi-coordinate system chain calibration, three-stage hybrid optimization inverse kinematics solution, path planning combining RRT and Bezier smoothing and angle continuity optimization, sequential joint control and early braking strategy, and loading / unloading state machine control integrating multi-sensor feedback, this method achieves a comprehensive technical effect of high success rate, high accuracy, short computation time, feasible and smooth path, stable and reliable execution process, and highly automated loading and unloading process while meeting the adaptability requirements of harsh underground working conditions. Attached Figure Description

[0019] Figure 1 A flowchart of an automatic control method for an underground engineering work trolley robotic arm based on a hybrid optimization strategy, provided by the present invention; Figure 2 The present invention provides a structural block diagram of an automatic control system for an underground engineering operation trolley robotic arm based on a hybrid optimization strategy. Detailed Implementation

[0020] The principles and features of the present invention are described below with reference to the accompanying drawings. The examples given are only for explaining the present invention and are not intended to limit the scope of the present invention.

[0021] Figure 1 A flowchart of an automatic control method for a robotic arm of an underground engineering work trolley based on a hybrid optimization strategy, provided by the present invention, is shown below. Figure 1 As shown, the automatic control method includes: Step 1: Calculate the coordinate system transformation parameters to the coordinate system set by the robot arm's initial pose; based on the coordinate system transformation parameters, transform the coordinates of the target point to the coordinate system of the robot arm, and determine the target pose of the robot arm based on the coordinates of the target point.

[0022] Step 2: Define a joint state vector representing the joint movement state; construct the search interval of the joint state vector, construct the objective function and gradient update function for joint movement, and perform iterative calculation to obtain the convergent solution as the initial solution of the joint state vector; perform a local discrete search in the vicinity of the initial solution to obtain the optimal solution of the joint state vector as the target joint state.

[0023] Step 3: Using the initial joint state and the target joint state as boundary points for graph search, a tree structure is constructed in the joint space through RRT (Rapidly-exploring Random Tree) sampling. Combined with target bias sampling and rewire optimization, the joint trajectory is generated.

[0024] This invention provides an automatic control method for a robotic arm of an underground engineering work trolley based on a hybrid optimization strategy. By forming a synergy among multiple levels such as a unified coordinate system, hybrid optimization inverse solution, and optimized path planning, the success rate, accuracy, and reliability of the robotic arm in complex working conditions such as underground engineering are significantly improved. Example 1

[0025] Embodiment 1 provided by this invention is an embodiment of an automatic control method for an underground engineering work trolley robotic arm based on a hybrid optimization strategy, combined with... Figure 1 It can be seen that embodiments of this automatic control method include: Step 1: Calculate the coordinate system transformation parameters to the coordinate system set by the robot arm's initial pose; based on the coordinate system transformation parameters, transform the coordinates of the target point to the coordinate system of the robot arm, and determine the target pose of the robot arm based on the coordinates of the target point.

[0026] In one possible embodiment, the process of calculating the coordinate system transformation parameters to the coordinate system set with the robot arm's initial pose in step 1 includes: Step 101: Set an initial coordinate system field according to the site environment, and select three non-collinear points under the coordinate system field.

[0027] In practice, due to the unstable underground environment, the underground workers can set up the coordinate system field according to the site environment, and select the coordinates of three non-collinear points under this coordinate system field.

[0028] Step 102: The coordinates of three points in the mapping coordinate system map are obtained by using the lidar mounted on the underground engineering work trolley.

[0029] Step 103: Measure the change in position of the lidar from the starting pose of the robotic arm, and calculate the coordinate transformation parameters from coordinate system field to coordinate system base_link of the robotic arm based on the change.

[0030] Coordinate system calibration is performed based on the 3-point calibration method. The homogeneous transformation matrix is ​​obtained through calibration, which can automatically convert the object coordinates recognized by the camera into motion coordinates that the robotic arm can understand and execute. This achieves multi-level coordinate system transformation from field to map to base link. It can transform these 3 points from the coordinate system set by the operator to the coordinate system set by the starting pose of the robotic arm, that is, transform the coordinate information of the target point to the coordinate system of the robotic arm.

[0031] Step 2: Define a joint state vector representing the joint movement state; construct the search interval of the joint state vector, construct the objective function and gradient update function for joint movement, and perform iterative calculation to obtain the convergent solution as the initial solution of the joint state vector; perform a local discrete search in the vicinity of the initial solution to obtain the optimal solution of the joint state vector as the target joint state.

[0032] In one possible embodiment, the joint state vector in step 2 includes the joint position. And the physical boundaries of the joint.

[0033] Joint position , The angle of rotation of the joint is expressed in rad. The displacement of the translating / extensional joint is expressed in meters (m).

[0034] The physical boundaries of a joint include the upper limit of its position. and lower limit .

[0035] In practice, the joint state vector also includes metadata such as joint type, unit, and timestamp.

[0036] In one possible embodiment, the process of calculating the initial solution of the joint state vector goal_joint_state in step 2 includes: Step 201, using the initial values ​​of the joint state vector. Centered on the target joint, the search interval is constructed within the allowable range of each joint as follows: .

[0037] in, The parameters are set according to the joint type and the redundancy of the mechanism. For example, the parameters for rotary joints are in the range of several degrees to tens of degrees, and the parameters for telescopic joints are in the range of several centimeters.

[0038] In practice, the initial value The initial value of the joint state, start_joint_state, is preferred, or the solution that has converged in the previous cycle or the nearest solution is selected. A feasible initial point is generated by combining the joint limits. When there are multiple solutions, the optimal initial value is selected according to the criteria of "minimum distance from the starting joint state + minimum attitude error + minimum constraint penalty".

[0039] A larger step size s_c is used for rapid coverage. The end pose error and constraint penalty are calculated for each grid point to obtain the candidate set and the current optimal solution.

[0040] Step 202: Perform a coarse mesh search within the search interval with a step size s_c, calculate the end pose error and constraint penalty for each mesh point, and obtain the candidate set and the current optimal solution.

[0041] Step 203: Using the optimal solution obtained from the coarse grid search as the initial value, construct the objective function as follows: .

[0042] in, , , and These are the weighting coefficients. and The end position / attitude obtained from the forward solution, and The end position / attitude of the optimal solution obtained from the coarse-grid search. Penalties are used to indicate joint limitation / collision / singularity. This is the smoothing term for joint changes.

[0043] Step 204, set the gradient update function as follows: .

[0044] For the set update step size, Use a fixed small step size or backtracking line for search; The gradient vector; and The kth and k+1th generations respectively .

[0045] In practice, the gradient can be calculated using the Jacobian matrix (or numerical difference).

[0046] The objective function is minimized, and the update is based on the gradient update function. The update stops when any of the following conditions are met: the pose error is below a threshold, the objective function decreases insufficiently, or the number of iterations reaches the upper limit. The initial solution is then obtained. .

[0047] In one possible embodiment, the process of obtaining the target joint state (goal_joint_state) in step 2 includes: Step 205, Construct the initial solution The surrounding area is: ;in, .

[0048] Step 206: Perform a fine mesh search in the nearby region based on step size s_f, where s_f ≪ s_c, to eliminate numerical errors and improve repeatability; calculate the end pose error and constraint penalty for each mesh point, and output the target joint state goal_joint_state.

[0049] In practice, step 2 uses a three-stage hybrid optimization strategy (coarse mesh search + gradient descent + fine mesh search) to solve the inverse kinematics of the robotic arm and outputs the target joint state / goal_joint_state.

[0050] Step 3: Using the initial joint state and the target joint state as boundary points for graph search, a tree structure is constructed in the joint space through RRT sampling. Combined with target bias sampling and rewire optimization, the joint trajectory is generated.

[0051] In one possible embodiment, step 3 includes: Step 301: Using the initial joint state `start_joint_state` and the target joint state `goal_joint_state` as boundary conditions, obstacle information `obstacles` is introduced, and RRT is used to generate a feasible joint path `joint_path`. The feasible joint path `joint_path` is then persisted as a usable path file. The feasible joint path `joint_path` includes: a discrete waypoint sequence { }, Time parameters { } / Sampling period and constraints { }

[0052] In practice, obstacle information can be obtained from environmental models or sensor fusion. The constraints {} in the feasible joint path are optional.

[0053] In one possible embodiment, step 301 further includes: making the feasible joint path (joint_path) meet the joint velocity / acceleration limit and continuity requirements by rewiring and Bezier smoothing.

[0054] The available path file is an Excel file for the execution layer to read.

[0055] Step 302: The controller reads the available path file and interpolates the discrete waypoint sequence {} into the reference trajectory. .

[0056] In practice, linear / spline interpolation can be used to ensure the continuity and amplitude limitation of velocity and acceleration.

[0057] Step 303, the calculation error is: ; The actual trajectory is determined based on the current pose (current_pose), feedback from each joint encoder, and the actual travel length (extension_length) of the extension joint.

[0058] Step 304: The controller outputs control quantity for each joint using a closed-loop strategy of feedforward + PID (including saturation and anti-integral saturation) based on the error; when a limit switch, abnormal load or tracking error exceeds the threshold is detected, the controller enters a degraded or safe shutdown process to ensure system safety.

[0059] In one possible embodiment, in step 304, the control quantities include: joint drive command control_value, joint enable / emergency stop status joint_enable_status, joint forward and reverse direction joint_direction, and extension joint target stroke extension_length_command (including soft limit protection and position determination).

[0060] In one possible embodiment, step 3 is followed by: combining multi-sensor information with host computer control commands to drive the actuator to perform operations, thereby achieving an automated process for the lever's advance and retraction.

[0061] The drive actuator includes: rod take-up arm, pusher, retainer and rotating rod, etc.

[0062] The multi-sensor information includes at least: joint encoder angle, extension stroke, limit switch status, attitude / tilt (IMU), and environmental / obstacle detection results (for updating / obstacles).

[0063] The host computer commands should include at least: target hole position / target pose (used to form target_pose), operation mode, target depth setting, allowable error threshold and safety boundary, etc.; operation modes include: positioning, drilling, rod retraction, pause and emergency stop.

[0064] The controller calculates the target parameters for the current stage based on information from multiple sensors and control commands from the host computer, and sends them down to the lower-level drive actuators in the order of the state machine to achieve continuous, safe, and traceable automatic operation. The target parameters include: target pose, propulsion amount, and speed / force control setpoint.

[0065] In an embodiment of an automatic control system for an underground engineering work trolley robotic arm based on a hybrid optimization strategy provided by this invention, the `coordinate_transform` package is used to achieve a complete transformation chain from field to map to base_link by utilizing CSV calibration points and encoder angles, and employing optimization methods such as SVD decomposition. The calibration error is controlled within the 1mm range. This unifies the field coordinates to the robotic arm's base coordinate system, providing high-precision target pose for subsequent planning. It also enables multi-coordinate system chain calibration and real-time coordinate transformation.

[0066] The inverse_arm_planner employs a three-stage hybrid optimization approach to solve the inverse kinematics: 1. Coarse-grid search (approximately 10,800 candidate solutions) to obtain an initial solution; 2. Iterative gradient descent with a driving term (0.4) and an adaptive learning rate (≤500 iterations); 3. Fine-grid search (21... 3=9,261 groups) were used for final refinement. The success rate of inverse kinematics was increased to 95%+, the position error was <10 mm, and the time was 50-60 ms, which is significantly better than the conventional single algorithm.

[0067] Path planning is performed using a combination of RRT*, Bezier, and angle continuity. The `arm_rrt_planner` optimizes path planning in joint space using RRT* sampling and rewiring based on target bias; it also introduces Bezier curve smoothing and incorporates angle continuity correction to avoid ±π jumps. Finally, the path is persistently stored in both topic and Excel files, achieving decoupling and reproducibility between planning and execution.

[0068] This path tracking control employs sequential joint enable, PID control, and pre-braking. The arm_controller operates at 100Hz. Sequential joint enable allows for joint-by-joint execution, reducing coupling and impact. Each joint is controlled independently by PID. A pre-braking strategy based on speed estimation, combined with independent PID control of the extension / retraction length, improves terminal positioning accuracy, avoids overshoot, and enhances execution smoothness.

[0069] The drilling_control system employs a multi-state machine for automatic control of the rod-taking production line. It utilizes a finite state machine with 19 states for rod advance and 18 states for rod retraction, integrating feedback from multiple sources such as pressure sensors, encoders, and distance sensors. This achieves complete automation of the rod advance / retraction production line at 20Hz, transforming complex manual operations into repeatable and monitorable automated processes.

[0070] The five-package layered architecture and ROS2 topic topology form an end-to-end data flow and control flow closed loop, encompassing perception (coordinate transformation), planning (inverse kinematics + path), control (path tracing), task execution (rod retrieval pipeline), and communication decoupling (persistence via topics and Excel).

[0071] Each package can be run, tested, and replaced independently, offering significant advantages in engineering implementation and maintenance. Example 2

[0072] Embodiment 2 provided by this invention is an embodiment of an automatic control system for an underground engineering work trolley robotic arm based on a hybrid optimization strategy. Figure 2 This invention provides a structural diagram of an automatic control system for an underground engineering work trolley robotic arm based on a hybrid optimization strategy, combined with... Figure 2As can be seen, the embodiment of this automatic control system adopts a layered and progressive architecture, including: a perception and coordinate transformation layer, a motion planning layer, an execution control layer, and a task execution layer. The perception and coordinate transformation layer is composed of the `coordinate_transform` package, which is responsible for chained coordinate transformations between multiple coordinate systems; the motion planning layer is composed of the `inverse_arm_planner` and `arm_rrt_planner` packages, which respectively implement inverse kinematics solution and path planning; the execution control layer is composed of the `arm_controller` package, which is used for closed-loop tracking control of joint paths; the task execution layer is composed of the `drilling_control` package, which serves as an independent rod-picking pipeline task module; each layer is coupled through explicit input / output interfaces and ROS2 topics, forming a complete link from "pose space → joint space → path space → control space → execution space".

[0073] The perception and coordinate transformation layer calculates coordinate transformation parameters to a coordinate system set based on the robot arm's initial pose; based on these parameters, it transforms the target point's coordinates into the robot arm's coordinate system, and then determines the robot arm's target pose based on these coordinates. In the perception and coordinate transformation layer, `coordinate_transform` reads the external CSV calibration file and the encoder angle (` / encoder_angle`), and uses algorithms such as SVD decomposition to perform a precise transformation from the field coordinate system to the base_link coordinate system, outputting the target pose (` / target_pose`) in a unified coordinate system. The perception and coordinate transformation layer mainly implements a three-stage hybrid optimization inverse kinematics solution. The initial guess optimization unit performs a mesh search near the initial guess value, generating a set of candidate values ​​for the joint angles that the robotic arm needs to adjust, and selects the optimal initial guess based on the cost function evaluation. The gradient descent optimization unit uses momentum terms to accelerate convergence based on the gradient information of the cost function, and iteratively optimizes the joint angles by adjusting the adaptive learning rate. The core of this method is to improve computational speed while ensuring accuracy.

[0074] The motion planning layer is used to define the joint state vector representing the joint movement state; construct the search interval of the joint state vector, construct the objective function and gradient update function of the joint movement, and perform iterative calculation to obtain the convergent solution as the initial solution of the joint state vector; perform local discrete search on the vicinity of the initial solution to obtain the optimal solution of the joint state vector as the target joint state.

[0075] In the motion planning layer, `inverse_arm_planner`, based on ` / target_pose` and ` / start_pose`, implements the path planning layer for the robotic arm using the RRT* algorithm. Sampling is randomly configured, with target bias sampling performed every five iterations, a step size of 0.1 rad, and a maximum of 1000 iterations. The tree grows to find feasible paths near the target. Bezier curves are used to smooth the path and optimize angular continuity, inserting 10 intermediate points in each path segment to make the path smoother. An additional requirement is that each joint has a corresponding movable angle; for example, the last joint of the robotic arm cannot move more than 180°, so the path with the least cumulative change is selected.

[0076] The core functions of the sequential joint control unit include sequential joint enabling, PID control, and pre-braking. It enables each joint sequentially, uses a PID controller to calculate the control output, and estimates pre-braking based on the angular or linear velocity of each joint's movement. The next joint is enabled only after each joint reaches its target position.

[0077] The execution control layer uses the initial and target joint states as boundary points for graph search. It constructs a tree structure in joint space through RRT sampling and generates joint trajectories by combining target bias sampling and rewire optimization. Within the execution control layer, the arm_controller reads ` / joint_path` from the Excel file.

[0078] The task execution layer is used to combine information from multiple sensors with control commands from the host computer to drive the actuators to perform tasks.

[0079] The core function of the pole loading and unloading task execution layer is state machine control combined with sensor feedback fusion. The 19 states during the pole loading process are executed in a state machine loop 14 times. Similarly, the 18 states during the pole unloading process are executed in a state machine loop 14 times. Before each transition to the next state, the system checks the sensors to confirm the correctness of the state. This streamlines the pole loading and unloading process.

[0080] In an embodiment of the automatic control system for the robotic arm of an underground engineering work trolley based on a hybrid optimization strategy provided by the present invention, the data flow structure and principle include: Structure Description: Starting from the pose space where / target_pose resides, it is mapped to the joint space ( / start_joint_state, / goal_joint_state) through inverse_arm_planner; then / joint_path and Excel path files are generated in the path space by arm_rrt_planner; after that, it is converted into control variables such as / control_value, / joint_enable_status, and / joint_direction in the control space in arm_controller; finally, it is applied to the physical robotic arm joints, enters the execution space, and forms a closed loop through position and other feedback.

[0081] Explanation of the principle: The principle of pose-to-joint transition: The inverse_arm_planner uses a three-stage hybrid optimization process to transform the geometrically defined target pose into a joint angle solution that satisfies the kinematic constraints of the robotic arm. This process traverses a wide range of candidate solutions in the coarse mesh search stage, quickly approaches the local optimum in the gradient descent stage, and further improves accuracy and success rate in the fine mesh search stage.

[0082] The principle of joint-to-path conversion: arm_rrt_planner uses the initial and target joint states as boundary points for graph search, constructs a tree structure in joint space through RRT sampling, and generates an approximately optimal joint trajectory by combining target bias sampling and rewire optimization. It then uses Bezier curves to smooth the trajectory and correct for angular continuity.

[0083] The principle of path-to-control conversion: The arm_controller calculates the control input for each joint using a PID controller based on the discrete joint angle sequence in the path and the current feedback state. It also employs sequential enable and early braking strategies to ensure gradual deceleration before reaching the target point and avoid overshoot.

[0084] Action Relationship Explanation: When a new / target_pose is generated, the data stream is automatically advanced from pose space to joint space; Each / joint_path generated by inverse kinematics and path planning will be converted into a series of timing control instructions; Feedback from the execution results (such as / current_pose) acts in reverse on the control algorithm, correcting the control input for the next step and forming a data-driven closed-loop action relationship.

[0085] In an embodiment of the automatic control system for the robotic arm of an underground engineering work trolley based on a hybrid optimization strategy provided by the present invention, the relationship between topic communication and room action includes: Structural Description: `coordinate_transform` subscribes to ` / encoder_angle` and publishes ` / target_pose`; `inverse_arm_planner` subscribes to ` / target_pose` and ` / start_pose`, and publishes ` / goal_joint_state` and ` / start_joint_state`; `arm_rrt_planner` subscribes to ` / start_joint_state`, ` / goal_joint_state`, and ` / obstacles`, and publishes ` / joint_path`, ` / pose_path`, and the path Excel file; `arm_controller` loads the path Excel file and subscribes to ` / current_pose` and ` / extension_length`, publishing ` / control_value`, ` / joint_enable_status`, ` / joint_direction`, and ` / extension_length_command`; `drilling_control` independently subscribes to multiple `sensors / *` and ` / control / *` command topics, and publishes control results topics such as lever arm status, thruster position, stabilizer status, and drum rotation.

[0086] Explanation of the principle: This invention employs the ROS2 topic mechanism to achieve loosely coupled communication between packets, with each packet only coupled through topic name and message type: through a topic publish / subscribe mode, calculation results or sensing data are broadcast in the form of messages; any packet can receive data as long as it subscribes to the corresponding topic, realizing decoupling and reuse between modules; the independent module drilling_control achieves task-level control of the rod-picking pipeline without directly interfering with motion planning and execution control logic through topic communication with the upper control and sensors.

[0087] Action relationship description: When coordinate_transform publishes a new / target_pose, inverse_arm_planner automatically triggers inverse kinematics solving; after inverse_arm_planner publishes a new joint state, arm_rrt_planner starts executing path planning; after arm_controller detects a new path Excel file or path topic, it starts path tracking control; drilling_control drives a series of state transitions in the advance or retract state machine based on received start / stop commands and sensor feedback.

[0088] This invention provides an embodiment of an automatic control system for an underground engineering work trolley robotic arm based on a hybrid optimization strategy. The boom retrieval assembly line working mode includes: Structural Description: The state machine structure of the feed pipeline and retraction pipeline in the drilling_control package: The entry line includes a multi-state process (21 states in total) from the first shot inspection, fixing, and advancing to 14 cycles; the exit line includes a multi-state process (18 states in total) from the last shot inspection, advancing, and guiding to 14 cycles.

[0089] Explanation of the principle: Each state corresponds to a set of control commands for the actuators (rod lifting arm, pusher, retainer, rotary drum, etc.) and judgment conditions for the sensors. The state machine determines whether the completion conditions of the current state are met based on sensor feedback (pressure, encoder, distance, etc.). If the conditions are met, the state machine transitions to the next state; otherwise, it maintains or performs a safety action. By using a finite state machine, the complex rod lifting process is solidified into a repeatable logical sequence.

[0090] Action Relationship Explanation: In rod advance mode, the system sequentially executes actions such as "first rod positioning → fixing → advancing → arm release → retraction → rotating drum → fixing again," completing the advancement of one rod in each cycle. In rod retraction mode, the system sequentially executes actions such as "last rod advancement → guidance → fixing release → retraction → drum rotation," retrieving the drill rod one by one. The transitions between each state are strictly determined by sensor feedback and upper-level control commands, ensuring that each mechanical action has clear trigger and termination conditions, thereby guaranteeing operational safety and reliability.

[0091] This invention provides an embodiment of an automatic control system for an underground engineering work trolley robotic arm based on a hybrid optimization strategy. The cooperative principle includes: The coordinate_transform package includes modules for calibration data management, encoder reading, and coordinate transformation. It achieves high-precision pose transformation through SVD decomposition and dynamic coordinate system fusion, providing a unified benchmark for subsequent planning.

[0092] The inverse_arm_planner package includes three types of algorithm modules: initial guess generation, gradient optimization, and fine search. Through the principle of hybrid optimization, it improves the success rate and accuracy of inverse solutions while satisfying constraints.

[0093] The arm_rrt_planner package contains modules for RRT sampling, rewiring, Bezier smoothing, and angle continuity correction. It outputs a feasible and smooth path through sampling search and trajectory smoothing principles.

[0094] The arm_controller package contains modules for trajectory analysis, sequential enable, PID control, and pre-braking. It achieves precise control of the robotic arm's motion through closed-loop control and predictive braking principles.

[0095] The drilling_control package is structured as a multi-state finite state machine, combining multi-source sensor inputs and actuator outputs. Through the state machine control principle, it decomposes the complex rod-taking process into an ordered sequence of discrete actions.

[0096] As can be seen from the above structural, principle, and action relationship descriptions, this invention achieves synergy across multiple levels, including a unified coordinate system, hybrid optimization inverse kinematics, optimized path planning, closed-loop control, and complex task state machines, thereby significantly improving the success rate, accuracy, and reliability of the robotic arm under complex working conditions. It is understood that the automatic control system for an underground engineering work trolley robotic arm based on a hybrid optimization strategy provided by this invention corresponds to the automatic control methods for underground engineering work trolley robotic arms based on hybrid optimization strategies provided in the aforementioned embodiments. The relevant technical features of the automatic control system for underground engineering work trolley robotic arms based on hybrid optimization strategies can be referenced from the relevant technical features of the automatic control methods for underground engineering work trolley robotic arms based on hybrid optimization strategies, and will not be repeated here.

[0097] This invention provides an automatic control method and system for an underground engineering work trolley robotic arm based on a hybrid optimization strategy. It organically combines non-camera multi-coordinate system chain calibration, three-stage hybrid optimization inverse kinematics solution, path planning combining RRT and Bezier smoothing and angle continuity optimization, sequential joint control and pre-braking strategies, and a loading / unloading state machine control incorporating multi-sensor feedback. This achieves a comprehensive technical effect of high success rate, high accuracy, short computation time, feasible and smooth paths, stable and reliable execution, and a highly automated loading / unloading process, while meeting the adaptability requirements of harsh underground working conditions. Therefore, although alternative solutions exist, the technical solution provided by this invention has superior overall engineering applicability and technical advantages under the specific application environment and performance constraints of this invention.

[0098] It should be noted that the descriptions of each embodiment in the above embodiments have different focuses. For parts that are not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

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

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

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

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

[0103] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0104] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. An automatic control method for a robotic arm of an underground engineering work trolley based on a hybrid optimization strategy, characterized in that, The automatic control method includes: Step 1: Calculate the coordinate system transformation parameters to the coordinate system set with the robot arm's initial pose; based on the coordinate system transformation parameters, transform the coordinates of the target point to the robot arm's coordinate system, and determine the target pose of the robot arm based on the coordinates of the target point; Step 2: Define a joint state vector representing the joint movement state; construct the search interval of the joint state vector, construct the objective function and gradient update function for joint movement, and perform iterative calculation to obtain the convergent solution as the initial solution of the joint state vector; perform a local discrete search in the vicinity of the initial solution to obtain the optimal solution of the joint state vector as the target joint state. Step 3: Using the initial joint state and the target joint state as boundary points for graph search, a tree structure is constructed in the joint space through RRT sampling. Combined with target bias sampling and rewire optimization, the joint trajectory is generated. The process of calculating the initial solution of the joint state vector goal_joint_state in step 2 includes: Step 201, using the initial value of the joint state vector. The search interval is constructed around the center as follows: ； in, These are parameters set according to joint type and mechanism redundancy. and These are the upper and lower limits of the joint position, respectively; Step 202: Perform a coarse mesh search in the search interval with a step size s_c, calculate the end pose error and constraint penalty for each mesh point, and obtain the candidate set and the current optimal solution; Step 203: Using the optimal solution obtained from the coarse grid search as the initial value, construct the objective function as follows: ； in, , , and These are the weighting coefficients. and The end position / attitude obtained from the forward solution, and The end position / attitude of the optimal solution obtained from the coarse-grid search. Penalties are used to indicate joint limitation / collision / singularity. For joint change smoothing; Step 204, set the gradient update function as follows: ； The set update step size; The gradient vector; and The kth and k+1th generations respectively ; With the objective function as the goal, update based on the gradient update function. The update stops when the pose error is below a threshold, the objective function decreases insufficiently, or the number of iterations reaches the upper limit, thus obtaining the initial solution. ; Step 3 includes: Step 301: Using the starting joint state `start_joint_state` and the target joint state `goal_joint_state` as boundary conditions, obstacle information `obstacles` is introduced, and a feasible joint path `joint_path` is generated using RRT. The feasible joint path `joint_path` is then persisted as a usable path file. The feasible joint path `joint_path` includes: a discrete waypoint sequence { }, Time parameters { } / Sampling period and constraints { }; Step 302: The controller reads the available path file and interpolates the discrete waypoint sequence {} into the reference trajectory. ; Step 303, the calculation error is: ; The actual trajectory is determined based on the current pose (current_pose), feedback from each joint encoder, and the actual travel length (extension_length) of the extension joint. Step 304: The controller outputs control quantity for each joint using a feedforward + PID closed-loop strategy based on the error; when a limit switch, abnormal load, or tracking error exceeding the threshold is detected, the controller enters a degraded or safe shutdown process.

2. The automatic control method according to claim 1, characterized in that, The process of calculating the coordinate system transformation parameters to the coordinate system set by the robot arm's initial pose in step 1 includes: Step 101: Set an initial coordinate system field according to the site environment, and select three non-collinear points under the coordinate system field; Step 102: The coordinates of the three points in the mapping coordinate system map are obtained by using the lidar mounted on the underground engineering work vehicle. Step 103: Measure the change in position of the lidar from the starting pose of the robotic arm, and calculate the coordinate system transformation parameters from the coordinate system field to the coordinate system base_link of the robotic arm based on the change.

3. The automatic control method according to claim 1, characterized in that, The joint state vector in step 2 includes joint position. and joint physical boundaries; The joint position , This refers to the angle of the rotating joint; The displacement of the locating / telescalating joint; The physical boundaries of the joint include the upper limit of the joint position. and lower limit .

4. The automatic control method according to claim 1, characterized in that, The process of obtaining the target joint state (goal_joint_state) in step 2 includes: Step 205, construct the initial solution The surrounding area is: ;in, ≪ ; Step 206: Perform a fine mesh search in the nearby region based on step size s_f, where s_f ≪ s_c; calculate the end pose error and constraint penalty for each mesh point, and output the target joint state goal_joint_state.

5. The automatic control method according to claim 1, characterized in that, Step 301 further includes: using rewire and Bezier smoothing to make the feasible joint path joint_path meet the requirements of joint velocity limiting / acceleration limiting and continuity; The available path file is an Excel file.

6. The automatic control method according to claim 1, characterized in that, In step 304, the control variables include: joint drive command control_value, joint enable / stop status joint_enable_status, joint forward and reverse direction joint_direction, and extension joint target stroke extension_length_command.

7. The automatic control method according to claim 6, characterized in that, After step 3, the method further includes: combining multi-sensor information with host computer control commands to drive the actuator to perform operations; The drive actuator includes: a lever arm, a pusher, a retainer, and a rotating rod; The multi-sensor information includes: joint encoder angle, extension stroke, limit switch status, attitude / tilt angle, and environmental / obstacle detection results; The host computer commands include: target hole position / target pose, operating mode, target depth setting, allowable error threshold and safety boundary; the operating modes include: positioning, drilling, rod retraction, pause and emergency stop; The controller calculates the target parameters for the current stage based on the multi-sensor information and the host computer control commands, and sends them to the lower-level drive actuators in state machine order; the target parameters include: target pose, propulsion amount, and speed / force control setpoint.

8. An automatic control system for a robotic arm of an underground engineering work trolley based on a hybrid optimization strategy, wherein the automatic control system is based on the automatic control method according to any one of claims 1-7, characterized in that, The automatic control system includes: a perception and coordinate transformation layer, a motion planning layer, an execution control layer, and a task execution layer; The perception and coordinate transformation layer is used to calculate coordinate system transformation parameters to transform to a coordinate system set with the robot arm's initial pose; based on the coordinate system transformation parameters, the coordinates of the target point are transformed to the robot arm's coordinate system, and the target pose of the robot arm is determined based on the coordinates of the target point; The motion planning layer is used to define a joint state vector representing the joint movement state; construct a search interval for the joint state vector; construct an objective function and gradient update function for joint movement; perform iterative calculations to obtain a converged solution as the initial solution of the joint state vector; and perform a local discrete search in the vicinity of the initial solution to obtain the optimal solution of the joint state vector as the target joint state. The execution control layer is used to take the initial joint state and the target joint state as boundary points of graph search, construct a tree structure in the joint space through RRT sampling, and generate joint trajectories by combining target bias sampling and rewire optimization. The task execution layer is used to combine multi-sensor information with host computer control commands to drive the execution mechanism to perform operations.

Images