Visual guidance robot efficient planning method and system for coal gangue sorting

Abstract

This invention discloses an efficient planning method and system for vision-guided robots in coal gangue sorting, comprising the following steps: establishing a kinematic model of the robotic arm and performing hand-eye calibration; calculating the expected picking pose based on the kinematic model and perception results, and analytically solving the joint space solution using IKFAST; employing a path planning strategy combining joint space linear interpolation and RRT-Connect, and solving for the collision-free optimal path based on a greedy algorithm and backward value iteration method; using a collision detection sampling method to improve detection efficiency; using the TOTG algorithm for time-optimal trajectory smoothing; constructing a time-series graph based on historical memory to query coal-carrying risk; and achieving continuous dynamic sorting through multi-dynamic target allocation and multi-thread scheduling. This invention can effectively compensate for the time delay between vision and execution, achieving high-precision, high-efficiency, and low-coal-carrying-risk automatic sorting of coal gangue.

Description

Technical Field

[0001] This invention relates to the field of mining machinery automation technology, and in particular to an efficient planning method and system for vision-guided robots for coal gangue sorting. Background Technology

[0002] my country is the world's largest coal producer, and coal has long dominated the primary energy structure, with widespread applications in industries such as power, steel, and building materials. Statistics show that coal accounts for approximately 56% of my country's primary energy consumption, and from 2017 to 2023, the national raw coal production showed an overall upward trend. For a considerable period, coal will remain the "ballast" for ensuring my country's stable energy supply and a "stabilizer" supporting energy structure adjustment and transformation. However, the sorting of coal gangue during coal mining and washing has long plagued the industry. Approximately 20% of the coal gangue needs to be sorted out for every ton of raw coal produced. If not sorted in a timely manner, this will reduce the calorific value and quality of the coal, affecting transportation efficiency and resource utilization, resulting in significant economic losses. Furthermore, the combustion of coal gangue releases toxic gases and smoke, severely polluting the atmosphere.

[0003] Coal gangue sorting is a crucial step in improving ore resource utilization and reducing environmental pollution. Traditional sorting methods mainly rely on manual operation or simple mechanical sorting. Manual sorting is not only inefficient and labor-intensive, but also involves complex working environments and poses high occupational health risks. Existing mechanized sorting methods, such as air separation, X-ray separation, and laser separation, generally suffer from drawbacks such as high equipment costs, complex technology, and poor adaptability, making it difficult to meet actual production needs in terms of coal gangue sorting efficiency. While physical detection methods such as X-rays and lasers have improved sorting accuracy to some extent, their high equipment cost, radiation safety hazards, and poor adaptability to working conditions have hindered their large-scale application in the coal industry.

[0004] With the rapid development of robotics and machine vision, vision-guided robotic sorting systems are gradually becoming efficient and precise solutions. However, current vision-based robotic arm grasping systems still face many technical bottlenecks in practical coal gangue sorting applications: the robotic arm has a small load capacity, slow grasping speed, and complex overall structure. Furthermore, it struggles to cope with the high diversity and complex working conditions of coal gangue in terms of shape, size, and surface characteristics, which restricts its sorting effect and operational reliability in actual production.

[0005] Therefore, developing an automatic coal gangue sorting technology that can balance efficiency, accuracy, adaptability, and economy is of great practical significance and engineering application value. Summary of the Invention

[0006] This invention aims to at least solve one of the technical problems existing in the prior art. To this end, this invention proposes an efficient planning method for a vision-guided robot for coal gangue sorting.

[0007] An efficient planning method for a vision-guided robot for coal gangue sorting according to an embodiment of the present invention includes the following steps: S1. Establish and calibrate the kinematic model of the robotic arm: Establish a kinematic model based on the parameters of the robotic arm to realize the mutual conversion between joint space and Cartesian space; perform hand-eye calibration by fixing the calibration plate to the end of the robotic arm and combining the camera's intrinsic and extrinsic parameters with the robotic arm's joint data. S2. Calculate the expected retrieval pose: Based on the forward kinematics model, inverse kinematics model, hand-eye calibration results, and camera perception results established in S1, obtain the expected retrieval pose of the robotic arm end effector in Cartesian space; solve the solution of the robotic arm in joint space using IKFAST analytically to ensure that the end effector can reach the expected retrieval pose. S3. Path planning: Path planning is performed using kinematic interpolation algorithm. Collision-free paths in joint space are calculated based on RRT-Connect algorithm. The optimal collision-free path is solved based on greedy algorithm and backward value iteration method. S4. Collision Detection: The robotic arm links, scene, coal, and coal gangue are simplified into geometric models using cylindrical envelope and convex hull methods, respectively. A collision detection method based on Decopitt sequence is adopted, which uses low-difference sequence to perform collision detection on the robotic arm configuration corresponding to each sampling point to determine whether there is a collision risk along the entire path. S5. Trajectory Smoothing and Optimization: Using the TOTG-based time-optimal trajectory planning algorithm, the entire motion trajectory is smoothed by optimizing the turning points on the path. S6. Coal Risk Inquiry: Construct a time-series graph based on historical memory, based on each sampling point planned in S5 and its corresponding timestamp, and combine the belt speed query to find the collision between coal and coal gangue. S7. Task scheduling and control system optimization: Through task scheduling algorithm, robot work tasks are allocated in real time according to the actual distribution of coal gangue and task priority. A multi-threaded continuous dynamic sorting algorithm is constructed to handle multiple coal gangue in the scene.

[0008] In some embodiments of the present invention, the hand-eye calibration in step S1 specifically includes: S11. Start the camera and obtain the camera intrinsic parameter matrix K; S12. Fix the calibration plate to the end of the robotic arm; S13. Control the robotic arm to move to multiple different postures within the camera's field of view, record the joint data of the robotic arm and acquire the calibration plate image in each posture, and call the hand-eye calibration algorithm to solve the hand-eye calibration matrix. S14. Convert the hand-eye calibration results into a transformation matrix from the world coordinate system to the camera coordinate system, perform nonlinear optimization by minimizing the average difference, and save the optimized calibration data. S15. If the calibration data results reach the preset number, proceed to step S16; otherwise, move to the new pose, resample, and proceed to step S13. S16. Calculate and sort the distances between each calibration result and the geometric mean result, and perform error analysis. S17. Determine whether the sorted distance is greater than the maximum allowed threshold. If so, delete the calibration data and return to step S16.

[0009] In some embodiments of the present invention, in step S2, the pose of the gangue after a certain time is predicted based on the current gangue pose, and the current gangue selection task trajectory is planned based on the prediction result to compensate for the time delay caused by the distance between the robotic arm and the camera.

[0010] In some embodiments of the present invention, step S3 further includes adaptive rock-picking insertion point pose calculation, as follows: S31. Obtain the image of the gangue in the pixel coordinate system, approximate the object contour using the convex hull, and obtain the centroid and outlet coordinates of the gangue. S32. Introduce the relaxation distance coefficient and magnification coefficient to calculate the distance between the coordinates of the toggle point and the coordinates of the center of gravity; S33. The center of gravity of the gangue in the base coordinate system of the robotic arm is obtained through the coordinate system transformation matrix. Then, the spatial coordinates of the turning point and the turning posture when turning the gangue are calculated to form a complete turning posture.

[0011] In some embodiments of the present invention, the path planning in step S3 includes joint space path planning and Cartesian space path planning; The joint space path planning is as follows: calculate the joint angle change for a given start and end point, perform interpolation to calculate the number of joint samples and the coordinates of the sampling points, perform collision detection on each sampling point, and output the sampling results in sequence if there is no collision; otherwise, use the RRT-Connect algorithm to plan the path in the joint space. The Cartesian space path planning is as follows: IKFAST is used to analytically solve for multiple solutions within the joint angle, a greedy algorithm is used to solve for the optimal joint value in the case of local collision-free conditions, and a backward value iteration method is used to solve for the globally optimal shortest path.

[0012] In some embodiments of the present invention, in step S4, the order of collision detection sampling points is determined based on the Decopitt sequence, so that each detection point maximizes the coverage of the undetected area.

[0013] In some embodiments of the present invention, in step S5, the TOTG algorithm is used to generate the time-optimal trajectory, and the non-differentiable points on the hypercircle smooth path are used to reduce the path in the configuration space to a one-dimensional arc length domain. Under the physical constraints of velocity and acceleration, the optimal trajectory is determined by forward integration and backward integration.

[0014] In some embodiments of the present invention, in step S6, by backtracking the timestamps and true poses of each sampling point in the expected trajectory to the time and position in the image coordinate system, the center path of the lever, the two lever paths, and the parallel paths of the two target object normal maximum edges are calculated, and the average sampling depth of the five trajectories is queried to obtain the coal carrying probability.

[0015] In some embodiments of the present invention, in step S7, after the system starts, the perception algorithm processes camera data and performs target detection to provide the coordinate position of the coal gangue in the pixel image; the coordinates are converted into the position in the robot arm base coordinate system through calibration results and robot arm parameters; priority is allocated through a multi-dynamic target allocation strategy; the time and position of the target object entering the workspace are obtained by combining the belt speed and the robot arm workspace, and the insertion point pose is calculated through the adaptive gangue-moving point calculation module; after successful path planning, environmental collision detection and belt coal risk detection are performed through the collision detection module; collision-free paths are merged into a collision-free trajectory; and timestamp security is ensured through a multi-threaded continuous planning execution algorithm to prevent time conflicts between threads.

[0016] This invention also mentions an efficient planning system for a vision-guided robot for coal gangue sorting. The system employs the above-mentioned method and includes a perception module, a planning module, and an execution module. The perception module is used to collect and process camera data and perform target detection. The planning module is used to perform kinematic modeling, hand-eye calibration, path planning, collision detection, trajectory optimization, coal risk query, and task scheduling. The execution module is used to control the robotic arm to perform sorting operations.

[0017] Compared with existing technologies, the present invention provides an efficient planning method for vision-guided robots in coal gangue sorting, which has the following advantages: First, this invention establishes a precise transformation relationship between the camera pixel coordinate system and the robotic arm base coordinate system through hand-eye calibration technology. Combined with forward and inverse kinematics models, it can convert the visually perceived pixel coordinates of coal gangue into spatial poses with millimeter-level precision. Simultaneously, addressing the spatial distance between the camera and the robotic arm and the time delay caused by belt conveyor transport, this invention proposes a dynamic gangue-picking algorithm. By predicting the gangue's pose at future moments, it effectively compensates for the time difference between image acquisition and robotic arm execution, ensuring that the robotic arm can pick up the gangue at the optimal moment when it enters the workspace, significantly improving sorting accuracy in dynamic unstructured scenarios. Second, this invention employs a path planning strategy combining joint-space linear interpolation and RRT-Connect. It prioritizes path generation using computationally efficient linear interpolation, only initiating a rapid expansion search when a collision occurs, thus balancing planning efficiency and success rate. Furthermore, based on the greedy algorithm's solution to local optima, this invention introduces a backward value iteration method for global optimization. The optimal path is selected with the goal of minimizing the total joint movement distance, effectively avoiding the problem of the greedy algorithm getting trapped in local optima and ensuring the global optimality of the planned path. Third, this invention employs a collision detection sampling method based on the Decopitt sequence, replacing the traditional sequential sampling method. The Decopitt sequence generates a uniformly distributed sequence of sampling points within the path arc length parameter range. Regardless of the obstacle's location on the path, it can be quickly detected with fewer sampling attempts, effectively avoiding the inefficiency of sequential sampling when facing obstacles further back, and significantly improving the real-time performance and computational efficiency of collision detection. Fourth, this invention employs the TOTG (Time-Optimal Trajectory Generation) algorithm, using non-differentiable points on a hypercircular smooth path to reduce the path dimension in configuration space to a one-dimensional arc length domain. Under the condition of satisfying physical constraints such as velocity and acceleration, the time-optimal trajectory is determined through forward and backward integration. This strategy effectively reduces the motion error of the robotic arm caused by path turning, eliminates stuttering and unnecessary vibration during execution, and minimizes the execution time of a single sorting task while ensuring smooth motion. Fifth, this invention constructs a time-series graph based on historical memory, retrospectively estimating the timestamps and actual poses of each sampling point in the planned trajectory back to the time and position in the image coordinate system. By calculating the center path of the lever, the two lever paths, and the parallel paths of the two maximum normal edges of the target object, the average sampling depth of the five trajectories is queried to quantitatively assess the probability of coal inclusion. This mechanism can predict and avoid the risk of potentially ejecting coal blocks during the trajectory planning stage, effectively reducing coal loss during gangue sorting. Sixth, this invention employs a multi-dynamic target allocation strategy to allocate robot tasks in real time based on the actual distribution of coal gangue and task priorities. Combining belt speed and the robotic arm's workspace, it predicts the time and location of each target object entering the workspace. Through a multi-threaded continuous dynamic sorting algorithm, the perception, planning, and execution modules are time-stamped to ensure security and prevent time conflicts between threads. This enables continuous sorting without start-stop in multi-target scenarios, avoiding time waste caused by task switching or waiting, and significantly increasing the sorting quantity per unit time. Seventh, the adaptive gangue insertion point pose calculation method of the present invention, by introducing a relaxation distance coefficient and a magnification coefficient, can calculate multiple sets of gangue insertion points and paths at the same gangue location, effectively adapting to gangue of different sizes and shapes. Simultaneously, the iterative planning and trajectory scaling mechanism can automatically adjust the planning strategy or linearly scale the trajectory when time constraints cannot be met, ensuring stable operation of the system under various working conditions and reducing the need for manual intervention. Eighth, this invention effectively reduces the gangue content in coal products through efficient and precise automatic sorting of coal gangue, thereby improving the calorific value and quality of coal and reducing transportation costs and resource waste. Simultaneously, by reducing the risk of missorting and carrying coal with the gangue, it lowers the emission of toxic gases and soot during the combustion of coal gangue, demonstrating significant environmental friendliness and social benefits. Attached Figure Description

[0018] Figure 1 This is a flowchart illustrating the efficient planning method for a vision-guided robot for coal gangue sorting mentioned in the embodiments of the present invention. Figure 2 This is an overall framework diagram of the vision-guided shovel robot system of the present invention; Figure 3 This is a flowchart of the multi-threaded, multi-dynamic target time-optimal algorithm in this invention; Figure 4 This is a schematic diagram of the path planning algorithm for each stage based on sampling in this invention; Figure 5 This is a schematic diagram of the adaptive rock-picking insertion point algorithm in this invention; Figure 6 This is a schematic diagram of the TOTG limit curve of the present invention; Figure 7 This is a schematic diagram of the time-optimal trajectory smoothing algorithm based on TOTG of this invention; Figure 8 This is a schematic diagram of the coal risk algorithm based on historical memory of the present invention; Figure 9 This is a schematic diagram of the working range of the robot of the present invention; Figure 10 This is a schematic diagram of the relaxation method used in this invention; Figure 11 This is a schematic diagram of a further relaxation method of the present invention; Figure 12 This is a schematic diagram of the greedy solution using IKFAST; Figure 13 This is a schematic diagram of the backward value iteration solution; Figure 14 This is a schematic diagram of the sequential sampling method; Figure 15This is a schematic diagram of the Van der Corput sampling method; Figure 16 This is a visual schematic diagram of the super-circular arc smoothing of the present invention. Detailed Implementation

[0019] Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0020] The following is for reference. Figures 1 to 16 A method for efficient planning of a vision-guided robot for coal gangue sorting according to an embodiment of the present invention is described, comprising the following steps: S1. Establish and calibrate the kinematic model of the robotic arm: Establish a kinematic model based on the parameters of the robotic arm to realize the mutual conversion between joint space and Cartesian space; perform hand-eye calibration by fixing the calibration plate to the end of the robotic arm and combining the camera's intrinsic and extrinsic parameters with the robotic arm's joint data. S2. Calculate the expected retrieval pose: Based on the forward kinematics model, inverse kinematics model, hand-eye calibration results, and camera perception results established in S1, obtain the expected retrieval pose of the robotic arm end effector in Cartesian space; solve the solution of the robotic arm in joint space using IKFAST analytically to ensure that the end effector can reach the expected retrieval pose. S3. Path planning: Path planning is performed using kinematic interpolation algorithm. Collision-free paths in joint space are calculated based on RRT-Connect algorithm. The optimal collision-free path is solved based on greedy algorithm and backward value iteration method. S4. Collision Detection: The robotic arm links, scene, coal, and coal gangue are simplified into geometric models using cylindrical envelope and convex hull methods, respectively. A collision detection method based on Decopitt sequence is adopted, which uses low-difference sequence to perform collision detection on the robotic arm configuration corresponding to each sampling point to determine whether there is a collision risk along the entire path. S5. Trajectory Smoothing and Optimization: Using the TOTG-based time-optimal trajectory planning algorithm, the entire motion trajectory is smoothed by optimizing the turning points on the path. S6. Coal Risk Inquiry: Construct a time-series graph based on historical memory, based on each sampling point planned in S5 and its corresponding timestamp, and combine the belt speed query to find the collision between coal and coal gangue. S7. Task scheduling and control system optimization: Through task scheduling algorithm, robot work tasks are allocated in real time according to the actual distribution of coal gangue and task priority. A multi-threaded continuous dynamic sorting algorithm is constructed to handle multiple coal gangue in the scene.

[0021] Specifically, in step S1, the kinematic model of the robotic arm can be established using the DH parameter method. Based on the link length, link rotation angle, link offset, and joint angle of the robotic arm, a coordinate system for each link is established, and a homogeneous transformation matrix between adjacent links is constructed. By multiplying these matrices, the total transformation matrix of the end effector relative to the base is obtained, thereby obtaining the forward kinematics solver. Based on the forward kinematics model, an inverse kinematics solver is constructed using analytical or numerical methods.

[0022] The hand-eye calibration in step S1 specifically includes: S11. Start the camera and obtain the camera intrinsic parameter matrix K; S12. Fix the calibration plate to the end of the robotic arm; S13. Control the robotic arm to move to multiple different postures within the camera's field of view. Record the joint data of the robotic arm and acquire images of the calibration board in each posture. Call the hand-eye calibration algorithm to solve for the hand-eye calibration matrix X, i.e., the transformation matrix. Among them, 48 different poses can be collected.

[0023] S14. Convert the hand-eye calibration results into a transformation matrix from the world coordinate system to the camera coordinate system, i.e. The calibration data was nonlinearly optimized by minimizing the average difference, and the optimized data was saved. ; S15. If the calibration data results reach the preset number, proceed to step S16; otherwise, move to the new pose, resample, and proceed to step S13. S16. Calculate and sort the distances between each calibration result and the geometric mean result, and perform error analysis. S17. Determine whether the sorted distance is greater than the maximum allowed threshold. If so, delete the calibration data and return to step S16.

[0024] Considering the considerable distance between the robotic arm and the camera, it takes some time for the camera to enter the robotic arm's workspace after it detects the gangue. Therefore, it is necessary to predict the gangue's pose after a certain period of time based on the current gangue pose, and plan the current gangue selection task trajectory based on the prediction results to compensate for the time delay caused by the distance between the robotic arm and the camera.

[0025] The time when the previous task planning was completed and the planned trajectory was transmitted to the controller was recorded. Total execution time of the previous trajectory This is the sum of the controller execution time, trajectory execution time, and message passing time. ,in, Controller execution time refers to the time from when the planning result is transmitted to the controller to when the controller begins execution; The execution time of the trajectory; The message transmission time refers to the time elapsed from the end of trajectory execution to the return of the trajectory execution completion status. .

[0026] At the same time, record the time when the camera captures the image. The position of the gangue at this moment is recorded as Let the time when the pose calculated by the image processing node is received be denoted as . The calculated position of the gangue is... ,and The true position of the gangue at any given time is .

[0027] Assumption 1: Belt speed The constant frictional forces during transport cause changes in the orientation of the gangue to be negligible.

[0028] Because the average mass of the gangue is over 20 kg, the image detection area is far from the gangue outlet, the probability of the gangue rolling is extremely low, and the conveyor belt runs smoothly with virtually no vibration. Under these assumptions, the formula describing the predicted pose of the gangue after a given time is: , , in, For mathematical constants, It is a direction vector. It's the belt speed. It is the direction vector of the belt running direction in the robot's base coordinate system.

[0029] Given an upper bound on the time taken to plan the task That is, the planning time for any task of selecting coal. ,in, This is the actual time required for planning. It is the waiting time so that the time for any planning can be a constant. .

[0030] Based on this, we introduce the following assumptions: Assumption 2: , All are fixed values; Assumption 3: The actual execution time of the trajectory is consistent with the planned time of the trajectory.

[0031] The two assumptions above are satisfied by the controller.

[0032] Within the planned trajectory, the time taken for the robot's end effector to travel from the starting position to the surface position is: The time taken from the starting position to the end of execution (i.e., the trajectory execution time) is And the minimum allowable time point at which the planning results should be transmitted to the controller is recorded. Therefore, the time point at which the pose of the gangue needs to be predicted in the dynamic gangue selection task is: , , Only in the above formula It is unknown.

[0033] As shown in Figure 7, assume The time is the expected moment when the claw contacts the gangue, and it should satisfy the condition that... The moment the waste rock enters the robot's working range, that is... .

[0034] yes The upper bound, which is in The projection of the conveyor belt surface satisfies: , , , , in, They are At the moment of impact, at the center of the waste rock, at the waste rock outlet, the projection of the conveyor belt velocity vector onto the conveyor belt surface, and... This represents the projection of a point on the outline of the gangue onto the surface of the conveyor belt. It is the working range of the robot on the conveyor belt surface, where the offset distance in the above formula is... have: , Therefore, we can conclude that: , What is requested That is, by offset distance Relaxed to the maximum center distance, supremum Relaxed to , Figure 10 This vividly illustrates the relaxation method.

[0035] Solving this problem requires solving an eighth-order polynomial, so further relaxation is needed to satisfy the conditions for a fast solution.

[0036] set up The moment when all the gangue has entered the working area, such as... Figure 11 As shown, since the conveyor belts can only operate in one direction within the working range, therefore, The following formula describes the state of gangue in... The state that should be satisfied at any time: .

[0037] The above formula can be used for quick calculation. Therefore, the timing for predicting the position of the gangue is... It should meet the following requirements: .

[0038] The top-level steps of the dynamic rock-picking algorithm are: S21, Prediction Realize the position of the waste rock at all times and plan a trajectory. The search within this trajectory yielded ; S22, if Then make If so, turn to S25; otherwise, turn to S23. S23, make (because so (The waste rock is still within the working range) Re-predict the waste rock's orientation and plan a new trajectory. And search results Turn onto S24; S24, if , then make Otherwise, for the new trajectory linear scaling The velocity acceleration is scaled accordingly by a factor of 1. , times; S25, Delayed waiting time The planning results are sent to the controller at all times.

[0039] It should be noted that the above-mentioned search based on the characteristics of the rock-digging scenario yielded the approximate time. The method can be summarized in the following steps: S231. Obtain the nearest time point saved during planning. The Cartesian coordinates of the end flange are used to preserve its height. ; S232, Query limit given time This refers to the total time of the planned trajectory and the lower limit of the time query. and specify intermediate values ; S233, Obtain The height at any given moment If it satisfies ,but If convergence occurs; otherwise, if If, then turn to S234; if Then turn onto S235; S234, Reassignment ,but ; then reassign the value And turn to S233; S235, Reassignment ,but ; then reassign the value And turn to S233.

[0040] Step S3 also includes adaptive rock removal insertion point pose calculation: Based on the camera perception results, the position of the target rock in the pixel plane and the maximum side length after convex hull encirclement are obtained. Based on the results of S1, the position of the rock removal exit is projected onto the pixel coordinate system, thereby calculating multiple sets of rock removal insertion points and paths at the same rock location. The specific steps are as follows: S31. Obtain the image of the gangue in pixel coordinates and approximate the object contour using a convex hull, such as... Figure 5 As shown, assume that the centroid of the gangue in the image coordinate system is... The convex hull is calculated using V-rep to determine the maximum contour. The exit coordinates in the image coordinate system are ,in , , These are the upper and lower boundary coordinates of the exit in the image coordinate system, respectively. S32. To ensure safe manipulation of the robotic arm's end effector, a relaxation distance coefficient is added. and magnification factor The distance between the coordinates of the pivot point and the coordinates of the centroid is: , Among them, the magnification factor ; This is the maximum distance from the edge of the gangue to its center of gravity, such as... Figure 5 middle As shown.

[0041] S33. The coordinate transformation matrix obtained after S1 can be used to obtain the center of gravity of the gangue in the robot arm's base coordinate system. Simultaneously, based on the coordinates of the rock-dispensing port in the robotic arm's base coordinate system... Then the two-dimensional vector in the image coordinate system for: , The spatial coordinates of the point where the gangue is moved are: , in, .

[0042] vector After the transformation from the image coordinate system to the end-effector coordinate system, the angle at which the robot arm rotates to the transformed vector position along the z-axis of the robot arm coordinate system is the towing posture. The coordinate plus the posture is the towing pose. .

[0043] The path planning algorithm in step S3; such as Figure 4 As shown, the purpose of robotic arm path planning is to plan a collision-free path from the initial pose to the target pose, given the end-effector pose and under constraints such as angle and angular velocity. After calculating the motion variables of each joint, these values ​​can be used as inputs to the robotic arm joint servo mechanism controller to achieve control of the robotic arm. Depending on the planning space, robotic arm path planning can be divided into joint space path planning and Cartesian space path planning.

[0044] Joint space path planning can be achieved by combining point-to-point path planning with RRT-Connect path planning. The specific steps are as follows: S311, For a given starting point and the end point Perform joint angle change calculations: , S312. Perform interpolation calculation to determine the number of samples for each joint: , in, It is a rounding function. To select the minimum sampling distance; S313. Calculate the coordinates of the sampling points: , Where i is , Let be the coordinates of the i-th sampling point.

[0045] S314. Collision detection is performed on each sampling point. If there is no collision, the sampling results are output in natural order, and the pose of the end effector of the robotic arm can be obtained by solving the forward kinematics. Otherwise, the RRT-Connect algorithm is used to plan the path of the robotic arm in the joint space.

[0046] Path planning in Cartesian space can be performed analytically using IKFAST. Since the IKFAST analytical solution method can obtain... Multiple solutions exist within the joint angle; a greedy algorithm can be used to find the optimal joint value in the case of local collision-free conditions, such as... Figure 12As shown, a local collision-free optimal solution does not necessarily represent the global optimal solution, and a local lack of solution may still lead to path planning failure.

[0047] Therefore, this invention optimizes the greedy solution by using a backward value iteration method to achieve the theoretical optimum, such as... Figure 13 As shown, let the distance in configuration space after solving the inverse kinematics for each sampling point be... Overall distance They are respectively: , , In the formula, valid represents no collision, invalid represents a collision, and the solution is... That is, the globally optimal shortest path.

[0048] S4. Collision Detection Algorithm: The collision detection algorithm is divided into collision detection between the robotic arm and environmental obstacles.

[0049] S3 path planning yields a series of sample points for the path. Checking each point individually would result in an infinite number of calls to the collision detection algorithm, which is impractical. Therefore, a common approach is to sample the path, perform collision checks on the sampled points, and set an appropriate step size to ensure the planned path is free of obstacles. Traditional sampling methods involve sampling sequentially, such as... Figure 14 As shown, detecting obstacles (red lines) consumes a significant amount of time and computational power when the obstacle is located in the latter half of the path.

[0050] To avoid this situation, the present invention uses a Decopitt-based sequence to maximize the coverage of undetected areas by each detection point, defined as follows: , in, ; , representing the i-th bit in the binary representation of the integer k, such as ,but , , ; That is to The binary bits are reversed.

[0051] As shown in Figure 15, the order of the detection points is calculated as follows: , Understandably, using collision detection sampling based on the Decopitt sequence is unaffected by the obstacle's position, and can detect obstacles even if they are far behind, greatly improving detection efficiency.

[0052] S5. Time-Optimal Trajectory Planning Algorithm: Given a path and robot physical constraints such as speed and acceleration limits, allocate arrival times for each waypoint to minimize the total time while satisfying the physical constraints. This invention uses the TOTG algorithm to generate time-optimal trajectories. This method uses hypercircles to smooth non-differentiable points on the path, such as... Figure 16 As shown in the figure For three adjacent sampling points in the path, For the path The angle at that point, To control the parameters of the circular arc approximation error, For the radius of the arc used for smoothing, For the center of the smooth arc, , Let be the direction vector for smoothing the arc. The hypercircle is defined as follows: , , , in, The error used to control the approximation of the circular arc is defined as follows: , in, It is the first robotic arm The position vectors of each joint. It can be understood that, under the above definition, the error of the circular arc approximation is always less than half the shorter segment of the piecewise straight line, and can be derived from... Hyperparameter control. Using this definition, previously non-differentiable path points become differentiable path points, and the entire trajectory becomes a G1 smooth path.

[0053] .

[0054] in, For the planned path function, that is, the one-dimensional parameters Mapped to points in a multidimensional configuration space , This can represent the current position reached by the path. To simplify the problem, the path in the configuration space is reduced to one dimension, i.e. , It is the path arc length. Based on this, the speed and acceleration limits defined in the time domain should be... This is transformed into first- and second-order differential limits defined over the arc-length domain. The transformation result is simply listed below, omitting the process.

[0055] , , , in, Indicates the speed of travel along the path. This represents the acceleration as the vehicle moves along the path. Represents the robot joint velocity vector. It can represent the joint acceleration vector of a robot. This represents the robot's degrees of freedom / number of joints. Indicates the robot in the The maximum speed allowed by the joint It can be said that the robot in the first... The maximum allowable acceleration of the joint, This indicates the maximum speed allowed by the path parameters. This can represent the maximum allowable acceleration of the path parameters. Indicates in The maximum permissible speed of a path determined solely by speed constraints. Indicates in The maximum permissible speed of the path is determined solely by acceleration constraints.

[0056] Based on the above understanding, the top-level TOTG algorithm is shown in the following flowchart: Step 1: Start from the beginning of the path, i.e. ; Step 2: At maximum acceleration The forward integral continues until one of the following three terms is satisfied: (1); , Turn to step five; (2) Turn to step four (3) Turn to step three; Step 3: Maintain maximum speed Until one of the following two conditions is met: (1) Turn to step two; (2) Turn to step four; Step 4: Continue searching for the next turning point along the combined limit curve. Turning step 5: Step 5: Integrate backward with minimum acceleration until it intersects with the forward trajectory. If this step is derived from Step 2, then the planning ends; otherwise, use the intersection point as the turning point to proceed to Step 4.

[0057] The above process can be found in [reference]. Figure 5 .

[0058] S6. Calculation of coal seam probability based on time series graph.

[0059] like Figure 8 As shown, after obtaining a trajectory of the robotic arm's end effector on the belt using S5, and the time mapping relationship in the dynamic bouldering algorithm of S2, the timestamps and true poses of each sampling point in the expected trajectory can be traced back to... The time and position of the target object in the image coordinate system are considered near the time of the target object. Previously, a collision-free picking path could be obtained through path planning, and the planned path on the conveyor belt, i.e., the path from the insertion point to the pick hole, was selected. Using the dimensions of the two-finger pick lever and the maximum distance from the edge of the rock to the center of gravity, the center path of the pick lever, two pick lever paths, and two parallel paths of the maximum normal edge of the target object can be calculated. The probability of coal inclusion can be obtained by averaging the five paths and performing a depth query. in, Let j represent the order of the sampling points, and j be the label of the five paths. For the j-th path Depth of each sampling point The total number of sampling points. It is the depth from the conveyor belt surface to the camera.

[0060] After the system starts, the perception algorithm processes camera data and performs target detection, providing the planning layer with the coordinates of the coal gangue in the pixel image. Using the calibrated results and relevant parameters of the robotic arm, the coordinates of the coal gangue in the pixel plane are converted into the position in the robotic arm's base coordinate system. A multi-dynamic target allocation strategy is used to assign priorities. Combining the belt speed and the robotic arm's workspace, the time and position of the target object entering the workspace are obtained. Then, the adaptive gangue-moving point calculation module calculates the pose of the insertion point. Path planning is performed using the path planning module and the calculated insertion point pose. (Failure...) Returning to the adaptive rock-moving point calculation module, the insertion point pose is recalculated. After successful path planning, the collision detection module is used. Collision detection is divided into two modules: one for obstacle collision detection, calculating whether each sampling point collides with the environment or target object; the other for whether the rock-moving path on the conveyor belt might carry coal. If a collision occurs, the process returns to the previous path planning module for replanning. If no collision occurs, the four collision-free paths are merged into a single collision-free trajectory, thus avoiding the inefficiency caused by the robotic arm's "start and stop" at path inflection points. Finally, the perception module, planning module, and execution module use a multi-threaded continuous planning and execution algorithm to ensure the security of timestamps and prevent time conflicts between threads.

[0061] In summary, this invention achieves millimeter-level precision dynamic target positioning and time compensation through hand-eye calibration and dynamic coal sorting algorithms, significantly improving sorting accuracy. It employs a path planning strategy combining linear interpolation and RRT-Connect, and introduces backward value iteration for global optimization, balancing planning efficiency and path optimality. By using Van der Corput sequences instead of traditional sequential sampling, it greatly improves the real-time performance and computational efficiency of collision detection. The TOTG time-optimal trajectory generation algorithm minimizes single sorting execution time while ensuring motion smoothness. A coal risk query mechanism based on time-series graphs effectively reduces coal loss. Combining multi-dynamic target allocation and multi-threaded scheduling enables continuous sorting without start-stop cycles, significantly increasing the sorting quantity per unit time. Adaptive insertion point pose calculation and trajectory scaling mechanisms enhance the system's adaptability to various operating conditions and reduce manual intervention. Ultimately, through efficient and accurate sorting, it improves the calorific value and quality of coal, reduces transportation costs and pollutant emissions, demonstrating significant environmental friendliness and socio-economic benefits.

[0062] This invention also mentions an efficient planning system for a vision-guided robot for coal gangue sorting. The system employs the above-mentioned method and includes a perception module, a planning module, and an execution module. The perception module is used to collect and process camera data and perform target detection. The planning module is used to perform kinematic modeling, hand-eye calibration, path planning, collision detection, trajectory optimization, coal risk query, and task scheduling. The execution module is used to control the robotic arm to perform sorting operations.

[0063] Understandably, the perception module acquires real-time images of coal gangue on the conveyor belt using an industrial camera, identifies and outputs the coordinates of the gangue in the pixel image using a deep learning object detection algorithm. After receiving these coordinates, the planning module first converts them into a spatial pose in the robotic arm's base coordinate system based on the hand-eye calibration matrix. Then, it uses a dynamic gangue-moving algorithm to predict the time offset of the gangue from being photographed to entering the robotic arm's workspace, performs time compensation on the pose, and obtains the expected moving pose. Subsequently, a path planning strategy combining linear interpolation and RRT-Connect is used to generate candidate paths, and the globally optimal path is selected through backward value iteration. The system employs a collision-free path and utilizes a Decopitt sequence-based rapid collision detection. The TOTG algorithm then performs time-optimal trajectory smoothing, while the system queries coal-carrying risks based on time-series graphs. Finally, a multi-dynamic target allocation and multi-threaded scheduling mechanism determines task priority and delivery timing. The execution module receives trajectory instructions from the planning module, controls the robotic arm's end effector to precisely pick up gangue at the optimal insertion point, and provides real-time feedback of the execution status to the planning module, forming a closed-loop control. These three modules work in parallel and collaboratively through a timestamp synchronization mechanism, enabling continuous sorting without start-stop operation in dynamic, unstructured scenarios.

[0064] This invention achieves fully intelligent sorting of coal gangue through deep collaboration among three modules: perception, planning, and execution. The system first uses a perception module to identify coal gangue using deep learning algorithms and outputs pixel coordinates. The planning module compensates for the time delay between vision and execution based on hand-eye calibration and dynamic gangue-moving algorithms, accurately predicting the future pose of the gangue. Subsequently, a path planning strategy combining linear interpolation and RRT-Connect is employed, along with backward value iteration for global optimization. Rapid collision detection is achieved using Decopitt sequences, and a time-optimal smooth trajectory is generated through the TOTG algorithm. Simultaneously, coal-carrying risks are queried based on time-series graphs. Finally, multi-threaded scheduling enables continuous sorting without start-stop operations, significantly improving sorting accuracy and efficiency in dynamic unstructured scenarios. This effectively reduces coal-carrying risks and mechanical motion errors, minimizes manual intervention and coal resource waste, and demonstrates significant environmental friendliness and industrial application value.

[0065] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims

1. An efficient planning method for a vision-guided robot for coal gangue sorting, characterized in that, Includes the following steps: S1. Establish and calibrate the kinematic model of the robotic arm: Establish a kinematic model based on the parameters of the robotic arm to realize the mutual conversion between joint space and Cartesian space; perform hand-eye calibration by fixing the calibration plate to the end of the robotic arm and combining the camera's intrinsic and extrinsic parameters with the robotic arm's joint data. S2. Calculate the expected retrieval pose: Based on the forward kinematics model, inverse kinematics model, hand-eye calibration results, and camera perception results established in S1, obtain the expected retrieval pose of the robotic arm end effector in Cartesian space; solve the solution of the robotic arm in joint space using IKFAST analytically to ensure that the end effector can reach the expected retrieval pose. S3. Path planning: Path planning is performed using kinematic interpolation algorithm. Collision-free paths in joint space are calculated based on RRT-Connect algorithm. The optimal collision-free path is solved based on greedy algorithm and backward value iteration method. S4. Collision Detection: The robotic arm links, scene, coal, and coal gangue are simplified into geometric models using cylindrical envelope and convex hull methods, respectively. A collision detection method based on Decopitt sequence is adopted, which uses low-difference sequence to perform collision detection on the robotic arm configuration corresponding to each sampling point to determine whether there is a collision risk along the entire path. S5. Trajectory Smoothing and Optimization: Using the TOTG-based time-optimal trajectory planning algorithm, the entire motion trajectory is smoothed by optimizing the turning points on the path. S6. Coal Risk Inquiry: Construct a time-series graph based on historical memory, based on each sampling point planned in S5 and its corresponding timestamp, and combine the belt speed query to find the collision between coal and coal gangue. S7. Task scheduling and control system optimization: Through task scheduling algorithm, robot work tasks are allocated in real time according to the actual distribution of coal gangue and task priority. A multi-threaded continuous dynamic sorting algorithm is constructed to handle multiple coal gangue in the scene.

2. The efficient planning method for a vision-guided robot for coal gangue sorting according to claim 1, characterized in that, The hand-eye calibration in step S1 specifically includes: S11. Start the camera and obtain the camera intrinsic parameter matrix K; S12. Fix the calibration plate to the end of the robotic arm; S13. Control the robotic arm to move to multiple different postures within the camera's field of view, record the joint data of the robotic arm and acquire the calibration plate image in each posture, and call the hand-eye calibration algorithm to solve the hand-eye calibration matrix. S14. Convert the hand-eye calibration results into a transformation matrix from the world coordinate system to the camera coordinate system, perform nonlinear optimization by minimizing the average difference, and save the optimized calibration data. S15. If the calibration data results reach the preset number, proceed to step S16; otherwise, move to the new pose, resample, and proceed to step S13. S16. Calculate and sort the distances between each calibration result and the geometric mean result, and perform error analysis. S17. Determine whether the sorted distance is greater than the maximum allowed threshold. If so, delete the calibration data and return to step S16.

3. The efficient planning method for vision-guided robots for coal gangue sorting according to claim 2, characterized in that, In step S2, the current position of the gangue is predicted based on the current position of the gangue, and the current gangue selection task trajectory is planned based on the prediction result to compensate for the time delay caused by the distance between the robotic arm and the camera.

4. The efficient planning method for vision-guided robots for coal gangue sorting according to claim 2, characterized in that, Step S3 further includes adaptive rock-picking insertion point pose calculation, as detailed below: S31. Obtain the image of the gangue in the pixel coordinate system, approximate the object contour using the convex hull, and obtain the centroid and outlet coordinates of the gangue. S32. Introduce the relaxation distance coefficient and magnification coefficient to calculate the distance between the coordinates of the toggle point and the coordinates of the center of gravity; S33. The center of gravity of the gangue in the base coordinate system of the robotic arm is obtained through the coordinate system transformation matrix. Then, the spatial coordinates of the turning point and the turning posture when turning the gangue are calculated to form a complete turning posture.

5. The efficient planning method for a vision-guided robot for coal gangue sorting according to claim 1 or 4, characterized in that, The path planning in step S3 includes joint space path planning and Cartesian space path planning. The joint space path planning is as follows: calculate the joint angle change for a given start and end point, perform interpolation to calculate the number of joint samples and the coordinates of the sampling points, perform collision detection on each sampling point, and output the sampling results in sequence if there is no collision; otherwise, use the RRT-Connect algorithm to plan the path in the joint space. The Cartesian space path planning is as follows: IKFAST is used to analytically solve for multiple solutions within the joint angle, a greedy algorithm is used to solve for the optimal joint value in the case of local collision-free conditions, and a backward value iteration method is used to solve for the globally optimal shortest path.

6. The efficient planning method for a vision-guided robot for coal gangue sorting according to claim 1, characterized in that, In step S4, the order of collision detection sampling points is determined based on the Decopitt sequence, so that each detection point maximizes the coverage of the undetected area.

7. The efficient planning method for a vision-guided robot for coal gangue sorting according to claim 1, characterized in that, In step S5, the TOTG algorithm is used to generate the time-optimal trajectory. Non-differentiable points on the hypercircle smooth path are used to reduce the path in the configuration space to a one-dimensional arc length domain. Under the physical constraints of velocity and acceleration, the optimal trajectory is determined by forward integration and backward integration.

8. The efficient planning method for a vision-guided robot for coal gangue sorting according to claim 1, characterized in that, In step S6, by backtracking the timestamps and true poses of each sampling point in the expected trajectory to the time and position in the image coordinate system, the center path of the lever, the two lever paths, and the parallel paths of the two target object's maximum normal edges are calculated. The average sampling depth of the five trajectories is then queried to obtain the probability of coal inclusion.

9. The efficient planning method for a vision-guided robot for coal gangue sorting according to claim 1, characterized in that, In step S7, after the system starts, the perception algorithm processes the camera data and detects the target to provide the coordinate position of the coal gangue in the pixel image; the coordinates are converted into the position in the robot arm base coordinate system through calibration results and robot arm parameters; and priority is allocated through a multi-dynamic target allocation strategy. By combining the belt speed and the workspace of the robotic arm, the time and position of the target object entering the workspace are obtained, and the insertion point pose is calculated through the adaptive bouldering point calculation module. After successful path planning, environmental collision detection and conveyor belt coal risk detection are performed through the collision detection module; collision-free paths are merged into a single collision-free trajectory; and a multi-threaded continuous planning execution algorithm is used to ensure timestamp security and prevent time conflicts between threads.

10. A high-efficiency planning system for a vision-guided robot in coal gangue sorting, characterized in that, The method according to any one of claims 1 to 9 includes a perception module, a planning module, and an execution module. The perception module is used to collect and process camera data and perform target detection. The planning module is used to perform kinematic modeling, hand-eye calibration, path planning, collision detection, trajectory optimization, coal risk query, and task scheduling. The execution module is used to control the robotic arm to perform sorting operations.

Images