Multi-modal perception-based coal bunker unblocking robot path planning and control method

Abstract

The invention discloses a path planning and control method for a coal bunker unblocking robot based on multi-modal sensing. Autonomous unblocking is achieved through three steps of cooperation of multi-modal sensing, double-layer path planning and a self-adaptive control strategy. The multi-mode sensing layer is fused with data of a binocular camera, a laser radar, UWB positioning, a force sensor and the like, and the volume, the position, the posture and the operation strength of a coal pile are accurately obtained; the double-layer path planning layer generates a global traversal path through an improved genetic algorithm based on a cleaning priority index, obtains a single-target local path through a fine trajectory of the global path, and updates in real time to cope with coal pile slipping; and the adaptive control strategy layer constructs a multi-target reward function, and adopts regularized closed-loop control to optimize parameters of an execution mechanism to form closed-loop control. According to the method, the problems that a traditional method cannot dynamically adapt to coal pile changes, and safety and efficiency are unbalanced are effectively solved, the problems that an existing unblocking device is rigid in path and solidified in control are solved, autonomous planning and dynamic adjustment can be achieved, and the unblocking efficiency is greatly improved.

Description

Technical Field

[0001] This invention belongs to the field of intelligent maintenance and robotics technology for coal bunkers in mines, specifically involving a path planning and control method for a coal bunker unblocking robot based on multimodal perception. Background Technology

[0002] Ground-level coal bunkers are crucial intermediate storage devices in coal mine production. Raw coal continuously flows in and out of the bunker. The upper part of the bunker is cylindrical, while the lower part is a hyperbolic cone. With a larger opening at the top and a smaller opening at the bottom, long-term operation is susceptible to factors such as coal adhesion, humid and hot environments, and wear and deformation of the bunker walls, leading to obstructed coal flow and coal pile blockages. Currently, most coal bunker clearing still relies on high-risk manual operations, including long-pole impacts and manual shoveling of coal. These methods are not only inefficient but also pose serious safety hazards such as coal block collapse and falls from heights. With the development of intelligent manufacturing and robotics, some coal bunker clearing equipment has adopted robotic arms for partial automation. However, existing solutions generally rely on limited sensing methods, making it difficult to model and update the complex and dynamically changing accumulation environment inside the coal bunker in real time. Path planning algorithms mostly depend on static environments and lack adaptability to coal collapse, channel changes, and irregular spaces, resulting in robot jamming and incomplete clearing. Summary of the Invention

[0003] The purpose of this invention is to provide a path planning and control method for a coal bunker clearing robot based on multimodal perception, which effectively solves the problem that traditional methods cannot dynamically adapt to changes in coal piles and the imbalance between safety and efficiency. It also overcomes the problems of rigid paths and fixed control in existing clearing equipment, enabling autonomous planning and dynamic adjustment, and significantly improving clearing efficiency.

[0004] To achieve the above objectives, this invention provides a path planning and control method for a coal bunker clearing robot based on multimodal perception. When a coal bunker is equipped with a coal bunker clearing robot, autonomous clearing is achieved through three steps: multimodal perception, two-layer path planning, and adaptive control strategy. The method includes the following steps: S1, Multimodal Sensing Layer: S1.1. Through the coordinated deployment of binocular vision cameras and lidar and the fusion analysis of multi-source data, high-precision acquisition of coal pile volume is achieved; S1.2 Deploy UWB positioning base stations, determine the spatial positioning of the coal pile and the silo opening and the real-time positioning of the robotic arm end effector, measure the working force of the robotic arm crushing shovel, and establish a spatiotemporal multi-dimensional perception dataset. S2, Two-layer path planning layer: S2.1 Based on the coal pile location and volume data obtained from the multimodal perception layer, calculate the coal pile cleaning priority index, generate the global path traversal order, and use the RRT* algorithm to generate local fine trajectory to adapt to the coal pile shape. S2.2 Real-time monitoring of coal pile slippage; recalculation of priority index when conditions are met; synchronous update of global and local paths to adapt to environmental changes. S3, Adaptive Control Strategy Layer: A multi-objective reward function is constructed, and the execution parameters are optimized through rule-based closed-loop control to drive the robot's operation and provide real-time feedback for adjustment, thus forming a closed-loop control.

[0005] As a further embodiment of the present invention: a binocular vision camera is installed on the top of the robot body, and a lidar is deployed below the binocular vision camera. The lidar and the binocular vision camera form a spatial complementary perspective. A circular scanning method is used to collect the distance point cloud of the coal pile surface: the binocular point cloud and the lidar point cloud; the binocular vision camera is controlled to output the pixel coordinates of the left and right views at a preset frame rate, and the lidar outputs the distance point cloud of the coal pile surface in polar coordinate form at a preset frequency. A global coordinate system is established with the robot's body center as the origin. The pixel coordinates of the binocular vision camera are converted into three-dimensional spatial coordinates through intrinsic and extrinsic parameters, and the polar coordinates of the LiDAR are converted into Cartesian coordinates. The iterative nearest point algorithm is used to align the binocular point cloud with the LiDAR point cloud to generate a dense coal pile three-dimensional point cloud model. The outer contour boundary of the coal pile three-dimensional point cloud model is extracted based on the three-dimensional convex hull algorithm, and the volume inside the convex hull is calculated by the tetrahedral subdivision method. The data timestamps of the binocular vision camera and the LiDAR are aligned through hardware trigger signals to form a spatiotemporally matched multi-source perception data set.

[0006] As a further aspect of the present invention, the method for achieving spatial positioning of the coal pile and the silo opening, and real-time positioning of the robotic arm's end effector, comprises the following specific steps: First, three UWB positioning base stations are evenly deployed around the perimeter of the coal bunker opening to form a triangular positioning network, with the base stations installed at the same height as the bunker opening plane. A UWB positioning tag is fixedly installed on the top of the coal bunker clearing robot. The UWB positioning tag is matched to the communication frequency band of the three UWB positioning base stations and is used to receive base station signals to calculate the robot's position. Joint encoders are installed at each joint of the robotic arm to collect the rotation angles of each joint in real time. An inertial measurement unit (IMU) is integrated at the end of the robotic arm. The IMU is used to measure the attitude angles of the end of the robotic arm, including roll angle α, pitch angle β, and yaw angle γ. Then, determine the calibration method of the reference coordinate system of the silo opening, the spatial positioning method of the coal pile and the silo opening, and the real-time positioning method of the end effector of the robotic arm; A laser rangefinder was used to measure the pairwise distances between three UWB positioning base stations, determining the triangular plane formed by the three base stations. A global positioning coordinate system was established with the geometric center of this triangular plane as the reference point O at the coal bunker entrance. The z-axis of the coordinate system runs vertically downwards along the coal bunker, and the xy-plane is parallel to the plane at the bunker entrance. The coordinates of the reference point O were set as (…). x 0 , y0 , z 0 ); Based on the three-dimensional point cloud model of the coal pile, calculate the centroid coordinates G of the coal pile. x g , y g , z g The straight-line distance D from the coal pile to the silo opening is obtained by using the spatial distance formula between the centroid coordinate G and the silo opening reference point O; A forward kinematics model of the robotic arm is established using the DH parameter method. The rotation angles collected by the encoders of each joint are substituted into the forward kinematics model of the robotic arm to calculate the theoretical position of the robotic arm's end effector. P theo ( x t , y t , z t The robot's UWB positioning tag receives signals from three UWB positioning base stations, and the actual position of the robot's center is calculated using a triangulation algorithm. P body ( x b , y b , z b ); construct the attitude rotation matrix R based on the attitude angles measured by the IMU. α , β , c ), for the theoretical position of the robotic arm end effector P theo Make corrections to obtain the actual position. P real ( x r , y r , z r ); The Extended Kalman Filter (EKF) algorithm is used to fuse high-frequency rotational data from the joint encoder with low-frequency, high-precision position data from UWB to determine the actual position P of the robotic arm's end effector. real Perform smoothing processing.

[0007] As a further aspect of the present invention: measuring the operating force of the robotic arm's breaker shovel and establishing a spatiotemporal multi-dimensional perception dataset, the specific steps are as follows: A six-dimensional force sensor is fixedly installed at the connection point between the end effector of the robotic arm and the breaker shovel; the six-dimensional force sensor is controlled to synchronously acquire three-dimensional force signals during the operation of the breaker shovel, including... Fx Horizontal radial force Fᵧ Horizontal circumferential force Fz Vertical impact, focusing on extracting key components directly related to the blockage clearing operation. Fz and F x / Fᵧ data; extract the maximum impact force of a single operation from the filtered effective force signal. F peak and the average work intensity per operation F avg , which serves as the core quantitative indicator of the breaker's operational strength; Based on the data timestamp alignment mechanism of binocular vision camera and lidar, the real-time position data of the robotic arm end of the extracted force quantification index and the three-dimensional point cloud model of coal pile are matched one by one to establish a spatiotemporal multi-dimensional perception dataset of "force-position-coal pile morphology".

[0008] As a further aspect of the present invention: based on the coal pile location and volume data obtained by the multimodal sensing layer, a coal pile cleaning priority index is calculated to generate a global path traversal order. The specific steps are as follows: Input the three-dimensional position information of each coal pile, and the centroid coordinates of each coal pile. G i ( x gi , y gi , z gi ), coal pile volume V i Current position information of the robotic arm: actual position of the robotic arm's end effector. P c ( x c , y c , z c ); Relationship between coal piles and silo openings: Straight-line distance from each coal pile to the silo opening reference point. D i Define a distance-to-the-bin weight function, and then apply it to each coal pile. i Calculate the cleanup priority index; With the dual objectives of "minimizing the total path length of the robotic arm traversing all coal piles and maximizing the overall cleaning priority completion rate," a global coarse path is generated; a fitness function is defined. F Prioritize including high-priority coal piles in the traversal path; initialize the path population based on the current position of the robotic arm. P c Starting from the coordinates of the centroid of all coal piles G iGiven the target point set, several sets of coal pile traversal orders are randomly generated, and the path with the highest fitness value is output as the global optimal path. Define the following global path constraints: j. The minimum distance between the path and the coal bunker wall must be ≥ the maximum cross-sectional radius of the robotic arm + 0.2m; k. An emergency escape passage with a width of ≥ 0.5m must be reserved between two adjacent coal piles; l. The path must not cross the solid area of ​​the coal pile, and the minimum distance between the path and the surface of each coal pile must be ≥ 0.3m.

[0009] As a further aspect of the present invention: the RRT* algorithm is used to generate local fine-grained trajectories to adapt to the coal pile morphology, and the specific steps are as follows: First, determine the information of the first target coal pile: the highest priority coal pile output by global path planning, and the 3D point cloud model of the coal pile; the current actual position of the robotic arm's end effector. P c The local outline of the coal bunker wall and the coordinates of the raised / depressed areas on the coal pile surface; based on the coordinates of the centroid of the first target coal pile. G 1 The three-dimensional contour point cloud is used to extract the safe working layer at a distance from the coal pile surface, and the first workable point of the target coal pile in the area is marked. Then, the modeling scope is defined using a local environment modeling method, based on the current position of the robotic arm. P c Coordinates of the first target coal pile's center of gravity G 1 Construct a local spatial model of a cube with a side length of , using the diagonal as the base. P c and G 1 The straight-line distance + 1m; An improved fast exploratory random tree algorithm (RRT*) is adopted, which is combined with the kinematic constraints of the robotic arm to achieve centimeter-level path planning; Define the following detailed path constraints: j. Path smoothness constraint, i.e., the turning angle of adjacent path segments ≤ 15°; k. Operation range constraint, i.e., all nodes on the path must be within the reachable range of the robotic arm end; l. Cleaning coverage constraint, i.e., the coverage density of the operation trajectory in the workable area of ​​the coal pile ≥ 2 points / cm².

[0010] As a further aspect of this invention: real-time monitoring of coal pile slippage, recalculation of priority index when conditions are met, synchronous updating of global and local paths to adapt to environmental changes, the specific steps are as follows: By aligning the binocular point cloud with the laser point cloud, the positional offset of each coal pile is detected in real time. ΔG i and volume change rate ΔV i ,when ΔG i >0.2m or ΔV i When the percentage is greater than 10%, it is considered a "potential slip"; if a "slip" has been detected or ≥2 coal piles are considered "potentially slipping", a global path update is immediately initiated; if a "potential slip", "slip" has occurred in the currently operating coal pile or the robotic arm's position deviates, the update will be triggered. ΔP When the value is greater than 0.05m, initiate a local path update; Reacquire and calculate the centroid coordinates of the fallen coal pile G i and volume V i ; Recalculate the cleaning priority index of all coal piles; Using the current real-time position of the robotic arm as a new starting point, take the updated coal pile position, volume and priority as input, and regenerate the globally optimal traversal order; For the current working coal pile, update the local environment model based on the latest point cloud data, add the bounding box of the protruding obstacle formed by the slide, re-mark the workable area, and remove the original work points that were covered by the slide. Define the constraints for path updates: j. After global path update, the deviation between the new path and the original path is ≤1m; k. The response time for local path replanning is ≤200ms. Establish a two-way linkage mechanism between global and local paths: j. After the local path is updated, if the actual position of the robotic arm deviates from the critical turning point of the global path by more than 0.5m, the global path is triggered for synchronous fine-tuning; k. When the global path is updated, the currently operating coal pile is retained as the local target, and only the traversal order of subsequent coal piles is adjusted.

[0011] As a further aspect of the present invention: constructing a multi-objective reward function, the specific steps of which are as follows: The current amount of coal cleared per unit time is calculated based on the change in coal pile volume. ΔV With time Δt The distance between the current position of the robotic arm's end effector and the target point of the path planning; equipment safety-related data, including the real-time impact load of the robotic arm's breaker shovel. F peak Impact load threshold F th Data related to silo wall protection, including the contact pressure F between the robotic arm and the silo wall. x / Fᵧ、Safe value of contact pressure with silo wall P safe ; Define a multi-objective reward function R The weighted sum of unblocking efficiency, equipment safety, and warehouse wall protection.

[0012] As a further aspect of the present invention: the execution parameters are optimized through rule-based closed-loop control, and the specific steps are as follows: Acquire adjustable parameters, including the motion control commands of the robotic arm, the impact frequency and impact force of the end-effector shredder; during actual operation, based on real-time sensing data and the multi-objective reward function R, call the basic parameters from the strategy library, and fine-tune them using the gradient descent method to maximize the multi-objective reward function R; In the closed-loop execution process, the robot controller converts the optimized actuator parameters into control signals to drive the robotic arm to move; when the cleaning priority index of all coal piles is... I i ≤0.1 I max The algorithm terminates when a manual termination command is received, and the actuator resets to its initial position.

[0013] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention reduces the equipment collision rate to below 1% and the secondary damage rate of the warehouse walls by 80% through force-visual fusion perception, path constraints, and emergency avoidance control.

[0014] This invention utilizes a cleanup priority index combined with an improved genetic algorithm to plan the globally optimal traversal order. It generates centimeter-level local trajectories and, combined with a real-time path update mechanism, ensures a cleanup coverage rate of ≥95%.

[0015] This invention uses a closed-loop linkage of "perception-planning-control" to update the environmental model, path planning and control parameters in real time, so that it can still operate stably even if there are deviations in the position of the coal pile. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the overall process structure of the present invention; Figure 2 This is a flowchart illustrating the signal transmission process of the present invention. Figure 3 This is a flowchart and framework diagram of the path planning process of this invention; Figure 4 This is a diagram illustrating the overall framework of the control command output of this invention. Detailed Implementation

[0017] The invention will now be further described with reference to the accompanying drawings.

[0018] like Figure 1 and Figure 2 As shown, the path planning and control method for a coal bunker clearing robot based on multimodal perception achieves autonomous clearing in the case of a coal bunker equipped with a coal bunker clearing robot through three steps: multimodal perception, two-layer path planning, and adaptive control strategy. This includes the following steps: S1, Multimodal Sensing Layer: S1.1. Through the coordinated deployment of binocular vision cameras and lidar, and the fusion analysis of multi-source data, high-precision acquisition of coal pile volume is achieved.

[0019] Specifically, a binocular vision camera is mounted on the top of the robot body. This high-temperature and dust-resistant binocular vision camera is used to acquire stereoscopic images of the coal pile. A lidar is deployed below the binocular vision camera, forming a spatially complementary perspective. It uses a circular scanning method to acquire distance point clouds on the surface of the coal pile: binocular point clouds and lidar point clouds. The binocular vision camera is controlled to output the pixel coordinates of the left and right views at a preset frame rate, and the lidar outputs the distance point clouds of the coal pile surface in polar coordinates (angle θ, distance d) at a preset frequency, completing coordinate unification and point cloud fusion, and calculating the volume of the coal pile. The data timestamps of the binocular vision camera and lidar are aligned through a hardware trigger signal to ensure that the synchronization error is ≤10ms, forming a spatiotemporally matched multi-source perception data set. A global coordinate system is established with the robot's center as the origin. The pixel coordinates of the binocular vision camera are converted into three-dimensional spatial coordinates using (focal length f, principal point coordinates (u0, v0)) and extrinsic parameters (rotation matrix R, translation vector T). The polar coordinates (θ, d) of the lidar are converted into Cartesian coordinates (x=d・cosθ, y=d・sinθ, z=0, with height information supplemented by binocular data interpolation). An iterative nearest-point algorithm is used to align the binocular point cloud with the lidar point cloud, generating a dense three-dimensional point cloud model of the coal pile (point cloud density ≥100 points / cm²). The outer contour boundary of the three-dimensional point cloud model of the coal pile is extracted based on the three-dimensional convex hull algorithm, and the volume inside the convex hull is calculated using the tetrahedral subdivision method. The calculation formula is as follows: ; in, o With the origin as the point, p i , p j , p k The vertices of the triangle on the convex hull surface are used, and the average value is taken through a sliding window to reduce the fluctuation in the volume calculation of dynamic coal piles.

[0020] S1.2 Deploy UWB positioning base stations, determine the spatial positioning of the coal pile and the silo opening, and the real-time positioning of the robotic arm end effector. Measure the operating force of the robotic arm's crushing shovel and establish a spatiotemporal multi-dimensional perception dataset.

[0021] Furthermore, the specific steps for achieving spatial positioning of the coal pile and silo opening, as well as real-time positioning of the robotic arm's end effector, are as follows: First, three UWB positioning base stations are evenly distributed around the perimeter of the coal bunker opening to form a triangular positioning network, with the base stations installed at the same height as the bunker opening plane. A UWB positioning tag is fixedly installed on the top of the coal bunker clearing robot. The UWB positioning tag is matched to the communication frequency band of the three UWB positioning base stations and is used to receive base station signals to calculate the robot's position. Joint encoders are installed at each joint of the robotic arm, preferably one joint encoder with a resolution ≥1024 lines per joint, for real-time acquisition of joint rotation angles θ1~θ6. An inertial measurement unit (IMU) with a sampling rate ≥100Hz is integrated at the end of the robotic arm. The IMU is used to measure the attitude angles of the end of the robotic arm, including roll angle α, pitch angle β, and yaw angle γ. Then, determine the calibration method of the reference coordinate system of the silo opening, the spatial positioning method of the coal pile and the silo opening, and the real-time positioning method of the end effector of the robotic arm; A laser rangefinder was used to measure the pairwise distances between three UWB positioning base stations, determining the triangular plane formed by the three base stations. A global positioning coordinate system was established with the geometric center of this triangular plane as the reference point O at the coal bunker entrance. The z-axis of the coordinate system runs vertically downwards along the coal bunker, and the xy-plane is parallel to the plane at the bunker entrance. The coordinates of the reference point O were set as (…). x 0 , y 0 , z 0 ), where z0=0 (the warehouse opening plane is the z=0 reference plane); Based on the three-dimensional point cloud model of the coal pile, calculate the centroid coordinates G of the coal pile. x g , y g , z g ), through all three-dimensional coordinate points within the point cloud ( x i , y i , z i The formula for finding the mean of is: ; Where N is the total number of points in the three-dimensional point cloud of the coal pile; the straight-line distance D from the coal pile to the silo opening is obtained through the spatial distance formula between the centroid coordinate G and the reference point O at the silo opening, that is: ; Using reference point O as the origin, output the coordinates of the centroid G in the global positioning coordinate system to determine the radial offset of the coal pile relative to the silo opening. x g , y g ) and depth ( z g ); A forward kinematics model of the robotic arm is established using the DH parameter method. The rotation angles collected by the encoders of each joint are substituted into the forward kinematics model of the robotic arm to calculate the theoretical position of the end effector (the center of the breaker blade installation). P theo ( x t , y t , z t The robot's UWB positioning tag receives signals from three UWB positioning base stations, and the actual position of the robot's center is calculated using a triangulation algorithm. P body ( x b , y b , z b ); construct the attitude rotation matrix R based on the attitude angles measured by the IMU. α , β , c The theoretical position of the robotic arm's end effector is determined by the following formula. P theo Make corrections to obtain the actual position. P real ( x r , y r , z r ): ; The Extended Kalman Filter (EKF) algorithm is used to fuse high-frequency rotational data from the joint encoder with low-frequency, high-precision position data from UWB (sampling rate ≥10Hz) to determine the actual position P of the robotic arm's end effector. real Smoothing is performed to ensure that the positioning error is ≤ ±0.05m.

[0022] Furthermore, such as Figure 3 As shown, the working force of the robotic arm's breaker shovel is measured, and a spatiotemporal multi-dimensional perception dataset is established. The specific steps are as follows: A six-dimensional force sensor is fixedly installed at the connection point between the end effector of the robotic arm and the breaker shovel; the six-dimensional force sensor is controlled to synchronously acquire three-dimensional force signals during the operation of the breaker shovel, including... F x Horizontal radial force Fᵧ Horizontal circumferential force Fz Vertical impact, focusing on extracting key components directly related to the blockage clearing operation. Fz and F x / Fᵧ data; extract peak values ​​from the filtered effective force signal. Fpeak (Maximum impact force of a single operation) and average value F avg (Average operating force per operation) serves as the core quantitative indicator of the operating force of the breaker shovel. Based on the data timestamp alignment mechanism of binocular vision camera and lidar, the real-time position data of the robotic arm end of the extracted force quantification index and the three-dimensional point cloud model of coal pile are matched one by one to establish a spatiotemporal multi-dimensional perception dataset of "force-position-coal pile morphology".

[0023] A unified timestamp is assigned to the three types of sensing data, and global synchronization is achieved through the clock signal of the robot controller to ensure that the volume, position, and force data at the same moment correspond one-to-one. All data are uniformly mapped to the established global coordinate system to ensure the consistency of the spatial coordinates of the coal pile volume, coal pile position, robotic arm position, and crushing shovel force, providing a unified data foundation for subsequent path planning and control strategies.

[0024] S2, Two-layer path planning layer: S2.1 Based on the coal pile location and volume data obtained from the multimodal perception layer, calculate the coal pile cleaning priority index, generate the global path traversal order, and use the RRT* algorithm to generate local fine trajectory to adapt to the coal pile shape.

[0025] Furthermore, based on the coal pile location and volume data obtained from the multimodal sensing layer, a coal pile cleaning priority index is calculated to generate a global path traversal order. The specific steps are as follows: Input the three-dimensional position information of each coal pile, and the centroid coordinates of each coal pile. G i ( x gi , y gi , z gi ), coal pile volume V i Current position information of the robotic arm: actual position of the robotic arm's end effector. P c ( x c , y c , z c ); Relationship between coal piles and silo openings: Straight-line distance from each coal pile to the silo opening reference point. D i Define a distance-to-the-bin weight function, and then apply it to each coal pile. i Calculate the cleanup priority index; ; ; in, D max This is the maximum vertical distance from the coal bunker opening to the bottom of the bunker. D i The smaller, W ( D i The larger the value of ), the range of its values ​​is (0,1]. L i The higher the value, the higher the priority of cleaning up the coal pile.

[0026] A constrained genetic algorithm is used to generate a global coarse path with the dual objectives of minimizing the total path length of the robotic arm traversing all coal piles and maximizing the overall cleaning priority completion rate; a fitness function is defined. F Prioritize including high-priority coal piles in the traversal path; construct and define the fitness function: ; in, L This represents the total length of the currently planned path. L max is the maximum possible path length for random traversal of all coal piles; is the sum of the cleaning priority indices of the coal piles included in the current path; is the sum of the cleaning priority indices of all coal piles; weight coefficients. α =0.4、 β =0.6, prioritizing the inclusion of high-priority coal piles in the traversal path; Initialize the path population, starting from the current position of the robotic arm. P c Starting from the coordinates of the centroid of all coal piles G i Given a target point set, several sets of coal pile traversal orders are randomly generated, each set forming an initial path; based on the fitness function... F Select individuals with high fitness values ​​and retain them for the next generation of the population; perform cross-recombination on the selected individuals to generate new individuals; randomly adjust the coal pile traversal order of some individuals to avoid the algorithm getting stuck in local optima; repeat the above operations until the number of iterations reaches a preset threshold (≥50 generations), and output the path with the highest fitness value as the global optimal path. Define the following global path constraints: j. The minimum distance between the path and the coal bunker wall must be ≥ the maximum cross-sectional radius of the robotic arm + 0.2m to avoid collisions between the robotic arm and the bunker wall; k. The path segment between two adjacent coal piles must reserve an emergency escape passage with a width ≥ 0.5m to accommodate sudden scenarios such as coal pile slippage; l. The path must not cross the solid area of ​​the coal pile, and the minimum distance between the path and the surface of each coal pile must be ≥ 0.3m to ensure the safe movement of the robotic arm.

[0027] Furthermore, the RRT* algorithm is used to generate local fine trajectories to adapt to the coal pile morphology. The specific steps are as follows: First, determine the information of the first target coal pile: the highest priority coal pile output by global path planning, and the 3D point cloud model of the coal pile; the current actual position of the robotic arm's end effector. P c The local outline of the coal bunker wall and the coordinates of the raised / depressed areas on the coal pile surface; based on the coordinates of the centroid of the first target coal pile. G 1 The three-dimensional contour point cloud is used to extract the spatial area 0.1~0.3m away from the coal pile surface as the safe working layer, and the first workable point of the target coal pile in this area is marked. Then, the modeling scope is defined using a local environment modeling method, based on the current position of the robotic arm. P c Coordinates of the first target coal pile's center of gravity G 1 Construct a local spatial model of a cube with a side length of , using the diagonal as the base. P c and G 1 The straight-line distance + 1m ensures coverage of the "robotic arm travel path + coal pile operation area"; An improved fast exploratory random tree algorithm (RRT*) is adopted, which is combined with the kinematic constraints of the robotic arm to achieve centimeter-level path planning; The path resolution is set to ≤5cm, the sampling step size to 0.05~0.1m, and the number of iterations to ≥100 to ensure path smoothness. Nodes are randomly sampled within the feasible region of the local environment model, and each sampled node needs to be verified by the robotic arm's forward kinematics model (verifying that the joint angles are within the preset range: shoulder joint ≤±90°, elbow joint ≤±120°, wrist joint ≤±180°). The parent nodes of the sampled nodes are selected by Euclidean distance, a path tree is constructed, and the generated initial path is pruned and optimized (removing redundant nodes) to ensure the shortest total path length and no collisions. In terms of operation trajectory optimization, the conical coal pile generates a spiral descent trajectory of "top edge → center", and the coal seam adhering to the silo wall generates a transverse serpentine scanning trajectory. The minimum distance between each point on the path and obstacles is calculated and must be ≥0.1m. If this is not met, the path is resampled and generated.

[0028] Develop detailed local path constraints: j. Path smoothness constraint, i.e., the turning angle of adjacent path segments is ≤15° to avoid impact caused by violent movement of the robotic arm joints; k. Operation range constraint, i.e. all nodes on the path must be within the reachable range of the robotic arm end; l. Cleaning coverage constraint, i.e. the coverage density of the operation trajectory in the workable area of ​​the coal pile is ≥2 points / cm² to ensure no cleaning blind spots.

[0029] S2.2 Real-time monitoring of coal pile slippage; recalculation of priority index when conditions are met; synchronous update of global and local paths to adapt to environmental changes.

[0030] Furthermore, real-time monitoring of coal pile slippage is performed. When conditions are met, the priority index is recalculated, and global and local paths are updated synchronously to adapt to environmental changes. The specific steps are as follows: By aligning the binocular point cloud with the laser point cloud, the positional offset of each coal pile is detected in real time. ΔG i (The difference between the current centroid coordinates and the previous time step) and the rate of change of volume ΔV i (The ratio of the current volume to the initial volume), when ΔG i >0.2m or D V i When the percentage is greater than 10%, it is considered a "potential slip"; if a "slip" has been detected or ≥2 coal piles are considered "potentially slipping", a global path update is immediately initiated; if a "potential slip", "slip" has occurred in the currently operating coal pile or the robotic arm's position deviates, the update will be triggered. ΔP When the value is greater than 0.05m, initiate a local path update; Reacquire and calculate the centroid coordinates of the fallen coal pile G i and volume V i Recalculate the cleanup priority index for all coal piles; ; in, D i ' This is the updated distance between the slipped coal pile and the silo opening. If the slippage causes the coal pile to move closer to the silo opening, W ( D i ' The distance to the warehouse entrance weight function increases adaptively.

[0031] Starting from the current real-time position of the robotic arm, the updated position, volume, and priority of the coal pile are used as input to regenerate the globally optimal traversal order. For the current coal pile, the local environment model is updated based on the latest point cloud data, and a new bounding box of the protruding obstacle formed by the sliding (coal block pile with a height > 0.1m) is added. The workable area is re-marked, and the original work points covered by the sliding are removed. Define the following constraints for path updates: j. After a global path update, the deviation between the new path and the original path should be ≤1m (to ensure the continuity of the robotic arm's movement); k. The response time for local path replanning should be ≤200ms (to avoid the accumulation of risks caused by the continuous slippage of the coal pile); l. All updated paths must meet the aforementioned obstacle avoidance, safety distance, and robotic arm kinematic constraints.

[0032] Establish a two-way linkage mechanism between global and local paths: j. After the local path is updated, if the actual position of the robotic arm deviates from the critical turning point of the global path by more than 0.5m, the global path is synchronously fine-tuned; k. When the global path is updated, the currently operating coal pile is given priority as the local target, and only the traversal order of subsequent coal piles is adjusted to reduce the invalid movement of the robotic arm.

[0033] S3, Adaptive Control Strategy Layer: A multi-objective reward function is constructed, and the execution parameters are optimized through rule-based closed-loop control to drive the robot's operation and provide real-time feedback for adjustment, thus forming a closed-loop control.

[0034] Furthermore, such as Figure 4 As shown, multi-objective reward function R The weighted sum of unblocking efficiency, equipment safety, and warehouse wall protection.

[0035] The current amount of coal cleared per unit time is based on the change in coal pile volume. ΔV With time Δt Calculation, that is: E = ΔV / Δt; The distance between the current position of the robotic arm's end effector and the target point of the path planning; equipment safety-related data, including the real-time impact load of the robotic arm's breaker blade. F peak Impact load threshold F th Data related to silo wall protection, including the contact pressure F between the robotic arm and the silo wall. x / Fᵧ、Safe value of contact pressure with silo wall P safe ; Construct the mathematical expression for the multi-objective reward function, namely: Where ω1+ω2+ω3=1 is a dynamic weighting coefficient that can be adaptively adjusted according to the work scenario; Quantification method for each sub-reward item: Congestion clearing efficiency reward R E : ; in, E max To preset the maximum amount of cleanup per unit time, R E ∈ (0,1], the higher the efficiency, the greater the reward value; Equipment safety reward R s : F peak ≤ F th ,but Rs =1- F peak / F th ;like F peak > F th ,but R s =- k ·( F peak - F th ). k This serves as a penalty coefficient to ensure that a negative reward is triggered when the impact load exceeds the limit; Warehouse wall protection reward R p If contact pressure P ≤ P safe ,but R p =1- P / P safe ,like P > P safe ,but R p =- m ·( F peak - F th ), m is the penalty coefficient, to avoid excessive stress on the warehouse walls.

[0036] Furthermore, the execution parameters of the rule-based closed-loop control are optimized, and the specific steps are as follows: Acquire adjustable parameters, including the motion control commands of the robotic arm and the impact frequency of the end effector shovel. f =1~5Hz), impact force (controlled by hydraulic valve opening, corresponding to 500~4000N); in actual operation, based on real-time sensing data and multi-objective reward function R, the basic parameters are called from the strategy library and fine-tuned by gradient descent method to maximize the multi-objective reward function R; In the closed-loop execution process, the robot controller converts the optimized actuator parameters into control signals (motor pulses, hydraulic valve opening commands) to drive the robotic arm to move; when the cleaning priority index of all coal piles is... I i ≤0.1 I max ( I max When the algorithm is initially set to the highest priority, or when a manual termination command is received, the algorithm terminates and the actuator resets to its initial position.

Claims

1. A coal bunker unblocking robot path planning and control method based on multi-modal perception, in the case of a coal bunker equipped with a coal bunker unblocking robot, characterized in that, Autonomous unblocking is achieved through a three-step strategy of multi-modal perception, double-layer path planning and adaptive control strategy, which includes the following steps: S1, multi-modal perception layer: S1.1, through the cooperative arrangement of binocular vision camera and laser radar and multi-source data fusion analysis, realize the high-precision acquisition of coal pile volume; S1.2, set up UWB positioning base station, determine the spatial positioning method of coal pile and bin mouth and the realization method of real-time positioning of mechanical arm end, measure the working intensity of mechanical arm breaking shovel, and establish a spatio-temporal multi-dimensional perception data set; S2, double-layer path planning layer: S2.1, based on the coal pile position and volume data obtained by the multi-modal perception layer, calculate the coal pile cleaning priority index, generate the global path traversal sequence, and generate the local fine trajectory to adapt to the coal pile shape by using RRT* algorithm; S2.2, real-time monitoring of coal pile sliding, recalculate priority index when conditions are met, update global and local path synchronously, and adapt to environmental changes; S3, adaptive control strategy layer: Construct multi-objective reward function, regularize closed-loop control optimization execution parameter, drive robot operation and real-time feedback adjustment, form closed-loop control.

2. The coal bunker unclogging robot path planning and control method based on multi-modal perception according to claim 1, characterized in that, The binocular vision camera is installed on the top of the robot body, and the laser radar is arranged below the binocular vision camera. The laser radar and the binocular vision camera form a spatial complementary view angle, and the ring scanning method is adopted to collect the coal pile surface distance point cloud: binocular point cloud and laser point cloud; the binocular vision camera outputs the pixel coordinates of left and right views at a preset frame rate, and the laser radar outputs the coal pile surface distance point cloud in polar coordinate form at a preset frequency; A global coordinate system is established with the center of the robot body as the origin, the pixel coordinates of the binocular vision camera are converted into three-dimensional space coordinates through intrinsic and extrinsic parameters, and the polar coordinates of the laser radar are converted into Cartesian coordinates; the iterative closest point algorithm is used to align the binocular point cloud and the laser point cloud, and a dense coal pile three-dimensional point cloud model is generated; based on the three-dimensional convex hull algorithm, the outer contour boundary of the coal pile three-dimensional point cloud model is extracted, and the volume inside the convex hull is calculated by tetrahedral subdivision method; the data timestamp alignment of binocular vision camera and laser radar is realized through hardware trigger signal, and a spatio-temporal matching multi-source perception data group is formed.

3. The multi-modal perception based bunker unblocking robot path planning and control method according to claim 2, characterized in that, The spatial positioning method of coal pile and bin mouth and the real-time positioning method of mechanical arm end are as follows: Firstly, three UWB positioning base stations are evenly arranged on the edge of the coal bin mouth to form a triangular positioning network, and the base station installation height is flush with the bin mouth plane; a UWB positioning tag is fixedly installed on the top of the coal bin unblocking robot body, the UWB positioning tag and the three UWB positioning base stations match in communication frequency band, which is used to receive base station signal to solve the robot body position; joint encoders are installed at each joint of the mechanical arm for real-time acquisition of joint angles; an inertial measurement unit IMU is integrated at the end of the mechanical arm, which is used to measure the attitude angle of the end of the mechanical arm, including roll angle α, pitch angle β and yaw angle γ; Then, the calibration method of bin mouth reference coordinate system, the spatial positioning method of coal pile and bin mouth and the real-time positioning method of mechanical arm end are determined. The laser range finder is used to measure the distance between each two of the three UWB positioning base stations to determine a triangle plane formed by the three positioning base stations; a global positioning coordinate system is established with the geometric center of the triangle plane as a bin opening reference point O, the z-axis of the coordinate system is along the vertical downward direction of the coal bin, the x-y plane is parallel to the bin opening plane, and the coordinates of the reference point O are set as (0, 0, 0) x 0 , y 0 , z 0 ); According to the three-dimensional point cloud model of the coal pile, the center of gravity coordinate G of the coal pile is calculated x g , y g , z g ); the straight-line distance D of the coal pile from the bunker mouth is obtained through the spatial distance formula of the center of gravity coordinate G and the bunker mouth reference point O; The positive kinematics model of the robot arm is established by D-H parameter method, and the rotation angle collected by the joint encoder is substituted into the positive kinematics model of the robot arm to calculate the theoretical position of the end of the robot arm P theo ( x t , y t , z t ); the UWB positioning tag of the robot body receives signals of three UWB positioning base stations, and the actual position of the center of the robot body is calculated by a triangulation algorithm P body ( x b , y b , z b ); a posture rotation matrix R is constructed according to the attitude angle measured by the IMU α , β , γ ), the theoretical position of the end of the robot arm is corrected P theo , and the actual position P real ( x r , y r , z r ) is obtained The extended Kalman filter (EKF) algorithm is adopted to fuse the high-frequency rotation angle data of the joint encoder and the low-frequency high-precision position data of the UWB, so that the actual position P of the end of the robot arm is smoothed. real is smoothed.

4. The multi-modal perception based bunker unblocking robot path planning and control method according to claim 2, characterized in that, The working intensity of mechanical arm breaking shovel is measured, and a spatio-temporal multi-dimensional perception data set is established, the specific steps are as follows: A six-dimensional force sensor is fixedly installed at the connecting part of the mechanical arm and the breaking shovel; a three-dimensional force signal during the operation of the breaking shovel is synchronously collected by controlling the six-dimensional force sensor, including F x : horizontal radial force, Fᵧ : horizontal circumferential force, Fz : vertical impact, and the Fz and F x / Fᵧ data directly related to the unblocking operation are extracted; the maximum impact force of a single operation and the average operation force of a single operation are extracted from the filtered effective force signal F peak F avg , as the core quantitative indicators of the breaking shovel operation force.​ Based on the data timestamp alignment mechanism of binocular vision camera and laser radar, the extracted force quantitative indicators, real-time position data of the robot arm end, and three-dimensional point cloud model of the coal pile are one-to-one corresponding, and the "force-position-coal pile shape" spatiotemporal multi-dimensional perception dataset is established.

5. The multi-modal perception based bunker unblocking robot path planning and control method according to claim 2, characterized in that, Based on the coal pile position and volume data obtained by the multi-modal perception layer, the coal pile cleaning priority index is calculated, and the global path traversal sequence is generated, with the following specific steps: Input the three-dimensional position information of each coal pile, and the centroid coordinates of each coal pile. G i ( x gi , y gi , z gi ), coal pile volume V i Current position information of the robotic arm: actual position of the robotic arm's end effector. P c ( x c , y c , z c ); Relationship between coal piles and silo openings: Straight-line distance from each coal pile to the silo opening reference point. D i Define a distance-to-the-bin weight function, and then apply it to each coal pile. i Calculate the cleanup priority index; A global rough path is generated with the dual objectives of "minimum total path length of the mechanical arm traversing all coal piles + highest total cleaning priority completion degree"; a fitness function is defined F , high-priority coal piles are preferentially ensured to be included in the traversal path; a path population is initialized with the current position of the mechanical arm P c as the starting point, and the center of gravity coordinates of all coal piles G i as the target point set, a number of coal pile traversal sequences are randomly generated, and the path with the highest fitness value is output as the global optimal path; Formulate global path constraints: j The minimum distance between the path and the coal bunker wall ≥ the maximum cross-sectional radius of the robot arm + 0.2m; k The path segment between the adjacent two coal piles needs to reserve an emergency escape channel with a width ≥ 0.5m; l The path should not pass through the coal pile entity area, and the minimum distance to the surface of each coal pile ≥ 0.3m.

6. The multi-modal perception based bunker unblocking robot path planning and control method according to claim 5, characterized in that, Generate a local fine trajectory to adapt to the coal pile shape using the RRT* algorithm, with the following specific steps: Firstly, the first target coal pile information is determined, and the priority of the global path planning output is the highest coal pile, the three-dimensional point cloud model of the coal pile; the current actual position of the end of the mechanical arm P c , the local contour of the coal bunker wall, the coordinates of the convex / concave area of the coal pile surface based on the first target coal pile barycentric coordinates G 1 three-dimensional contour point cloud, extract the safe operation layer from the coal pile surface, and mark the first target coal pile operable point in the region; Then the modeling range is defined by local environment modeling method, and the current position of the robot arm is taken as the center P c The straight line distance between the center of gravity of the first target coal pile and the center of gravity of the second target coal pile is 1 m G 1 The diagonal line is taken as the diagonal line of the cube, and a local space model of the cube is constructed, and the side length of the cube is 1 m P c The straight line distance between the center of gravity of the first target coal pile and the center of gravity of the second target coal pile is 1 m G 1 The straight line distance between the center of gravity of the first target coal pile and the center of gravity of the second target coal pile is 1 m Use the improved rapid exploration random tree algorithm RRT* to fuse the kinematic constraints of the robot arm and achieve centimeter-level path planning; Formulate local detailed path constraints: j Path smoothness constraint, i.e. the angle between adjacent path segments ≤ 15°; k Job range constraint, i.e. all nodes on the path need to be within the reachable range of the robot arm end; l Cleaning coverage rate constraint, i.e. the coverage density of the job trajectory in the coal pile workable area ≥ 2 points / cm².

7. The multi-modal perception based bunker unblocking robot path planning and control method according to claim 6, characterized in that, Real-time monitoring of coal pile sliding, recalculation of priority index when conditions are met, synchronous updating of global and local paths, adaptation to environmental changes, with the following specific steps: By aligning binocular point cloud and laser point cloud, the position offset of each coal pile is detected in real time ΔG i and volume change rate ΔV i When ΔG i >0.2m or ΔV i >10%, it is determined as "potential sliding"; when "sliding" or ≥2 coal piles "potential sliding" are detected, global path update is immediately started; when "potential sliding", "sliding" or mechanical arm position deviation of current working coal pile Δ P >0.05m, local path update is started; Recapture and calculate the center of gravity coordinates of the coal slide G i and volume V i ; recalculate the clean-up priority index of all coal piles; Take the current real-time position of the robot arm as the new starting point, and use the updated coal pile position, volume and priority as input to regenerate the global optimal traversal sequence; for the current job coal pile, update the local environment model based on the latest point cloud data, add the convex obstacle bounding box formed by sliding, recalibrate the workable area, and exclude the original work points covered by sliding; Formulate path update constraints: j After updating the global path, the deviation between the new path and the original path ≤ 1m; k The response time of local path re-planning ≤ 200ms; Determine the two-way linkage mechanism of global and local paths: j After updating the local path, if the actual position of the robot arm deviates from the key turning point of the global path by > 0.5m, trigger the synchronous fine tuning of the global path; k When updating the global path, preferentially retain the coal pile currently being worked as the local target, and only adjust the traversal sequence of subsequent coal piles.

8. The multi-modal perception based bunker unblocking robot path planning and control method according to claim 2, characterized in that, Multi-objective reward function R A weighted sum of the clogging efficiency, the equipment safety, and the bin wall protection.

9. The multi-modal perception based bunker unblocking robot path planning and control method according to claim 8, characterized in that, Optimize the execution parameters of the regularized closed-loop control, with the following specific steps: Get adjustable parameters, including robot arm motion control instructions, impact frequency and impact force of the robot arm end breaking shovel; during actual operation, call the basic parameters from the strategy library according to the real-time perception data and multi-objective reward function R, and fine-tune them through gradient descent method to maximize the multi-objective reward function R; Closed-loop execution process, the robot controller converts the optimized actuator parameters into control signals to drive the mechanical arm device to act; when the cleaning priority index of all coal piles I i ≤0.1 I max Or when the manual termination instruction is received, the algorithm is terminated, and the actuator is reset to the initial position.

Images