On-site mixed loading explosive truck hole searching and accurate hole aligning method and system

Abstract

The invention discloses an on-site mixed loading explosive truck hole searching and accurate hole aligning method which comprises the following steps: acquiring a design coordinate set of each blast hole in a to-be-loaded area, acquiring a tail end coordinate of the tail end of a mechanical arm under a geographic coordinate system, and establishing a mechanical arm base coordinate system by taking a base coordinate as an original point; the joint angle and the expansion and contraction amount of the mechanical arm are obtained through iterative inverse kinematics with constraints; controlling the mechanical arm to carry out coarse alignment iteration until the coarse deviation is smaller than a first threshold value; obtaining a fine positioning result of the hole center and the hole axis direction; calculating the fine deviation of the tail end hole aligning assembly relative to the hole center and the hole shaft; after a hole alignment completion signal is output, triggering insertion of a charging conduit and checking of a charging interlocking condition; and when the interlocking condition is met, the charging operation is executed. The GNSS RTK, the laser radar and the 3D structured light camera are fused, quick positioning and alignment of the blast hole are achieved, and the charging efficiency and safety are greatly improved.

Description

Technical Field

[0001] This invention relates to the field of loading calibration technology for mixed explosives vehicles, and in particular to a method and system for finding and accurately aligning holes in on-site mixed explosives vehicles. Background Technology

[0002] In large-scale blasting operations such as mining, water conservancy projects, and transportation infrastructure construction, on-site mixing explosives trucks have become core equipment due to their ability to continuously mix and load explosives. However, existing technologies have significant bottlenecks in the borehole positioning and alignment stages, severely restricting operational efficiency and safety.

[0003] Traditional operations rely heavily on manual labor, requiring workers to enter hazardous blasting areas and manually adjust the positions of robotic arms and charging guides repeatedly through visual observation. This not only involves high labor intensity but also exposes workers to safety risks such as falling rocks and explosion residue. As the scale of blasting expands (the number of blast holes in a single operation can reach hundreds to thousands), the accuracy (often exceeding 10cm error) and efficiency (3-5 minutes per hole alignment) of manual hole alignment are no longer sufficient to meet engineering requirements. Summary of the Invention

[0004] This invention aims to at least partially solve one of the technical problems in related technologies. To this end, one objective of this invention is to propose a method and system for on-site mixed explosive loading vehicle borehole location and precise borehole alignment, integrating GNSS RTK, lidar, and a 3D structured light camera to achieve rapid borehole positioning and alignment, significantly improving loading efficiency and safety.

[0005] Firstly, the present invention proposes a method for finding and precisely aligning holes in an on-site mixed explosives vehicle, the steps of which are as follows:

[0006] S1: Obtain the set of design coordinates for each borehole within the area to be charged, and establish a task queue;

[0007] S2: Deploy GNSS RTK reference stations at the work site, and set up a first mobile station and a second mobile station on the mixed explosives vehicle. The first mobile station is installed at the base of the robotic arm, and the second mobile station is installed at the hole-aligning assembly at the end of the robotic arm.

[0008] S3: Calculate the base coordinates of the robotic arm base in the geographic coordinate system using the base station and the first mobile station, calculate the end coordinates of the robotic arm end in the geographic coordinate system using the base station and the second mobile station, and establish the robotic arm base coordinate system with the base coordinates as the origin;

[0009] S4: For the target borehole, based on the design coordinates of the target borehole, the coordinate system of the robot arm base, and the kinematic model of the robot arm, the joint angles and extension of the robot arm are obtained by using constrained iterative inverse kinematics solution, so that the end-effector hole assembly reaches the pre-aligned pose near the target borehole.

[0010] S5: Start the laser scanning mechanism installed on the end-hole alignment component to obtain the surface point cloud near the target borehole, perform ground segmentation and outlier removal on the point cloud, extract the borehole candidate region from the remaining point cloud and obtain the coarse positioning result of the borehole center through circle fitting; calculate the coarse deviation of the end-hole alignment component relative to the borehole center based on the coarse positioning result, and control the robotic arm to perform coarse alignment iteration until the coarse deviation is less than the first threshold.

[0011] S6: Start the structured light depth camera installed on the end-hole alignment assembly to acquire the depth map and point cloud of the target borehole opening. Transform the point cloud to the coordinate system of the end-hole alignment assembly. Extract the ring edge point set in the opening area and use RANSAC out-of-point culling to fit the circle to obtain the opening center. Fit a cylindrical model in the point cloud facing downwards from the opening to obtain the borehole axis direction, thereby obtaining the precise positioning result of the opening center and the borehole axis direction. Calculate the precise deviation of the end-hole alignment assembly relative to the opening center and the borehole axis.

[0012] S7: When the precision deviation is greater than the second threshold, the control end micro-motion hole-setting mechanism performs a two-dimensional orthogonal translation in a plane perpendicular to the hole axis to compensate for the precision deviation; after each translation, S6 is repeated to update the precision deviation until the precision deviation is less than the second threshold and a hole-setting completion signal is output.

[0013] S8: After outputting the hole completion signal, trigger the insertion of the charging guide tube and the charging interlock condition verification; if the interlock condition is met, execute the charging operation; otherwise, enter the reversal process and trigger manual intervention prompt.

[0014] Preferably, in step S3: the observation data of the first rover station and the second rover station are time-stamped and smoothed, and the orientation results of the dual antennas installed on the robot arm base are used to determine the horizontal orientation of the base coordinate system, thereby completing the attitude transformation from the geographic coordinate system to the robot arm base coordinate system.

[0015] Preferably, in step S4, the constraints on the joint angles and extension of the robotic arm obtained by iterative inverse kinematics solution are: joint angle limit constraint, extension stroke limit constraint, and minimum safe distance constraint between the end effector and the vehicle body and the ground.

[0016] Preferably, step S5 involves extracting candidate regions for orifices and obtaining coarse localization results for the orifice center, which includes: searching for concave regions in the remaining point cloud after removing ground point clouds that satisfy height abrupt changes and decrease point density as candidate regions for orifices; extracting the boundary point set of the candidate regions for orifices; and performing circle fitting on the boundary point set to obtain the coordinates of the orifice center.

[0017] Preferably, step S6, which involves extracting the annular edge point set and fitting a cylindrical model, includes: extracting the annular region of the aperture within a preset height range from the point cloud output by the structured light depth camera, and extracting the edge points of the annular region as the annular edge point set; performing circle fitting on the annular edge point set after removing outliers using RANSAC to obtain the aperture center; and selecting the point cloud within a preset depth range below the aperture center and performing cylindrical model fitting to obtain the aperture axis direction.

[0018] Preferably, the end micro-motion orifice-aligning mechanism in step S7 includes an X-axis linear module, a Y-axis linear module, and a movable platform whose displacement is controlled by the X-axis linear module and the Y-axis linear module. The maximum translational stroke of the X-axis linear module and the Y-axis linear module is 0.3 meters to 0.6 meters. The axis of the drug delivery tube is coaxially set with the movable platform of the end micro-motion orifice-aligning mechanism, so that the drug delivery tube can achieve equidistant translation with the two-dimensional translation of the movable platform.

[0019] Preferably, the interlocking condition verification in step S8 includes: the continuous stable time of the hole completion signal is greater than the third threshold; the hole opening area in the depth map of the structured light depth camera is unobstructed and meets the insertion determination; the angle between the insertion direction of the loading guide and the hole axis is less than the fourth threshold; the vehicle is in a parked state and the outriggers are in a supported position; the personnel detection result of the danger zone is that no personnel have entered; if any of the conditions are not met, loading is prohibited and the retraction process is initiated.

[0020] Secondly, the present invention proposes an intelligent hole-finding and precise hole-aligning system for an on-site mixed explosives vehicle, applying any of the above-mentioned hole-finding and precise hole-aligning methods for on-site mixed explosives vehicles. The system includes:

[0021] GNSS RTK base station;

[0022] The first GNSS RTK mobile station is fixedly installed on the robotic arm base;

[0023] The second GNSS RTK mobile station is fixedly installed at the end of the robotic arm's hole-aligning assembly;

[0024] The robotic arm has its base fixedly connected to the vehicle body;

[0025] An end-mounted aperture assembly is fixedly connected to the end of a robotic arm. The end-mounted aperture assembly includes a laser scanning mechanism, a structured light depth camera, an end-mounted micro-motion aperture assembly mechanism, and a drug delivery tube mounting structure. The laser scanning mechanism and the structured light depth camera are jointly mounted on the movable platform of the end-mounted micro-motion aperture assembly mechanism. The drug delivery tube mounting structure is fixed on the movable platform, and the axis of the drug delivery tube is coaxial with the translation center of the movable platform.

[0026] The controller is communicatively connected to the first GNSS RTK mobile station, the second GNSS RTK mobile station, the robotic arm, the laser scanning mechanism, the structured light depth camera, and the end effector micro-motion hole-setting mechanism.

[0027] The beneficial effects of this invention are:

[0028] By deploying GNSS RTK reference stations on site and setting up mobile stations at the robot arm base and end effector respectively, the positioning and unified coordinate constraints of the vehicle-arm-end effector are realized. Then, the robot arm base coordinate system is established with the base coordinates as the origin and the pose is calculated, enabling the robot arm to quickly reach the pre-alignment near the hole according to the task queue. This reduces the time loss and error accumulation caused by repeated manual visual positioning and provides a stable spatial reference and continuous control entry for subsequent closed-loop hole alignment.

[0029] A layered perception link of coarse laser point cloud positioning and fine structured light depth positioning is adopted. First, the end laser scanning mechanism collects point cloud data on the ground near the borehole and completes ground segmentation, outlier removal and borehole candidate extraction. The coarse positioning result of the borehole center is obtained by circle fitting and the robotic arm is driven to iterate coarse alignment. Then, based on the coarse alignment, the structured light depth camera outputs depth map and point cloud, extracts the annular edge of the borehole, and obtains the borehole center by circle fitting after RANSAC outlier removal. At the same time, the borehole axis direction is obtained by fitting a cylindrical model. Thus, the positioning is further converged from only being near the borehole to a precise state in which the center and axis are aligned. Therefore, the robustness and consistency of positioning and alignment can still be maintained in complex blasting environments such as dust and terrain undulation.

[0030] The end-effector micro-motion orifice alignment mechanism is coaxially integrated with the charge delivery tube on a movable platform. A two-dimensional orthogonal translation within a stroke range of 0.3m to 0.6m is achieved through an X / Y orthogonal linear module, decoupling the final alignment compensation (last few tens of centimeters) from the movement of the large robotic arm. Combined with an iterative mechanism of detection-fine-tuning-re-detection, this rapidly eliminates precision deviations, thereby shortening the time required for single-hole alignment and reducing reliance on operational experience. Simultaneously, after alignment, interlocking verification conditions are introduced, including orifice stability, unobstructed orifice opening, insertion angle, parking outriggers, and personnel in hazardous areas. This transforms the high-risk manual close-range orifice alignment into a controlled, automated calibration process, significantly reducing the frequency and exposure time of personnel entering hazardous areas, thus simultaneously enhancing operational safety boundaries while improving efficiency. Attached Figure Description

[0031] In the attached diagram:

[0032] Figure 1 This is a flowchart of a method for finding and accurately aligning holes in an on-site mixed explosives vehicle, as proposed in this invention. Detailed Implementation Example 1:

[0033] Reference Figure 1 A method for locating and precisely aligning holes in a vehicle used for on-site mixing of explosives, comprising the following steps:

[0034] S1: Obtain the set of design coordinates for each borehole within the area to be charged, and establish a task queue;

[0035] S2: Deploy GNSS RTK reference stations at the work site, and set up a first mobile station and a second mobile station on the mixed explosives vehicle. The first mobile station is installed at the base of the robotic arm, and the second mobile station is installed at the hole-aligning assembly at the end of the robotic arm.

[0036] S3: Calculate the base coordinates of the robotic arm base in the geographic coordinate system using the base station and the first mobile station, calculate the end coordinates of the robotic arm end in the geographic coordinate system using the base station and the second mobile station, and establish the robotic arm base coordinate system with the base coordinates as the origin;

[0037] Specifically: the observation data of the first and second rover stations are time-stamped and smoothed, and the orientation results of the dual antennas installed on the robot arm base are used to determine the horizontal orientation of the base coordinate system, thereby completing the attitude transformation from the geographic coordinate system to the robot arm base coordinate system.

[0038] In this embodiment: In order to convert the positioning results in the geographic coordinate system into control quantities that the robotic arm can directly use, the position vector of the base point in the geographic coordinate system calculated by the base station and the first rover station is denoted as... The position vector of the endpoint reference point in the geographic coordinate system, calculated by the base station and the second rover, is denoted as... Simultaneously, the observation data from the first and second rover stations are time-stamped and a sliding window mean filter is used to suppress instantaneous jumps. Then, the horizontal heading angle of the base is obtained using the orientation results of the dual antennas installed on the robotic arm base. Using the heading direction as the base coordinate system Axial unit vector Using its counterclockwise orthogonal direction as Axial unit vector Using the vertical upward direction as Axial unit vector Thus, a rotation matrix is ​​constructed. And obtain the homogeneous transformation from the geographic coordinate system to the base coordinate system. Thus any geographic coordinate point The expression in the base coordinate system is: The output of this transformation and This will be directly used as the input for the next step of solving the robot arm's kinematics.

[0039]

[0040]

[0041]

[0042]

[0043]

[0044]

[0045] S4: For the target borehole, based on the design coordinates of the target borehole, the coordinate system of the robot arm base, and the kinematic model of the robot arm, the joint angles and extension of the robot arm are obtained by using constrained iterative inverse kinematics solution, so that the end-effector hole assembly reaches the pre-aligned pose near the target borehole.

[0046] Specifically, the constraints on the joint angles and extension of the robotic arm obtained by iterative inverse kinematics solution are: joint angle limit constraint, extension stroke limit constraint, and minimum safe distance constraint between the end effector and the vehicle body and the ground.

[0047] In this embodiment: First, a forward kinematics model of the robotic arm is established, and the joint variable vector of the robotic arm is set as follows: , The number of joint variables of the robotic arm is determined by using... The parameters yield the homogeneous transformation of each joint. Then the pose of the end-hole assembly reference point relative to the base coordinate system is:

[0048]

[0049] The translation component can be obtained from this pose. As the end position expression; then the target borehole design coordinates are... The coordinates obtained from the previous step are as follows: The essence of pre-alignment is to find a solution that makes the end position close to the desired position near the target hole. Joint variables Therefore, the objective function is constructed as follows:

[0050]

[0051] Iterative updates are used:

[0052]

[0053] in, Let Jacobian be the positional Jacobian matrix. Step size, This is the damping factor; to ensure the constraints are implemented at the executable level, after each iteration, [the following is applied]: Apply joint angle limit constraints, telescopic stroke limit constraints, and minimum safe distance constraints between the end-hole assembly and the vehicle body and the ground. Specifically, this can be achieved by truncating outbound components and introducing penalties for breaches of safe distances. To achieve stable convergence of the solution and avoid mechanical interference, wherein, The weighting coefficient for the penalty term. The minimum distance that the end-hole assembly needs to maintain between itself and the vehicle body, the ground, or an obstacle. In joint state The actual minimum distance is obtained; the joint angles and extensions output by the inverse kinematics model will be used to drive the robotic arm to rotate, pitch and extend, so that the end-effector hole assembly reaches the pre-aligned pose near the target borehole.

[0054] S5: Start the laser scanning mechanism installed on the end-hole alignment component to obtain the surface point cloud near the target borehole, perform ground segmentation and outlier removal on the point cloud, extract the borehole candidate region from the remaining point cloud and obtain the coarse positioning result of the borehole center through circle fitting; calculate the coarse deviation of the end-hole alignment component relative to the borehole center based on the coarse positioning result, and control the robotic arm to perform coarse alignment iteration until the coarse deviation is less than the first threshold.

[0055] Specifically, the process of extracting candidate regions for the orifice and obtaining coarse localization results for the orifice center includes: searching for concave regions in the remaining point cloud after removing ground point clouds that satisfy height abrupt changes and decrease point density as candidate regions for the orifice; extracting the boundary point set of the candidate regions for the orifice; and performing circle fitting on the boundary point set to obtain the coordinates of the orifice center.

[0056] In this embodiment: the original point cloud obtained by the laser scanning mechanism is denoted as... First, the point cloud is segmented into ground planes in the coordinate system of the hole assembly at the end, specifically by least-squares fitting of the ground plane. Based on this, it will satisfy The points are identified as ground points and removed, among which... This represents the distance from which a point is identified as a ground point. Simultaneously, statistical outlier filtering is performed on the remaining point cloud to obtain a purified point cloud. ; then in The search identifies concave regions with highly abrupt changes and decreasing point density as candidate orifice regions. Extract the boundary point set of the candidate region of the orifice. And perform circle fitting on the boundary point set, using the least squares form:

[0057]

[0058] in, This represents the projected coordinates of the set of boundary points at the orifice onto the horizontal plane. This represents the radius of the fitted circle. This represents the coordinates of the center of the circle obtained from the fitting.

[0059] The coarse positioning of the orifice center is obtained by solving the problem. Based on the coarse positioning results and the current position of the end reference point. Obtain the coarse deviation vector The horizontal component of the robot arm in the base coordinate system is used as the control input to drive the robot arm for coarse alignment iteration. In this embodiment, the first threshold is set to 0.08 meters. When the meter is measured, it is determined that the coarse hole matching meets the first hole matching condition, thus providing a stable local area for the fine hole matching stage.

[0060] S6: Start the structured light depth camera installed on the end-hole alignment assembly to acquire the depth map and point cloud of the target borehole opening. Transform the point cloud to the coordinate system of the end-hole alignment assembly. Extract the ring edge point set in the opening area and use RANSAC out-of-point culling to fit the circle to obtain the opening center. Fit a cylindrical model in the point cloud facing downwards from the opening to obtain the borehole axis direction, thereby obtaining the precise positioning result of the opening center and the borehole axis direction. Calculate the precise deviation of the end-hole alignment assembly relative to the opening center and the borehole axis.

[0061] Specifically: Extracting the annular edge point set and fitting a cylindrical model includes: extracting the annular region of the aperture within a preset height range from the point cloud output by the structured light depth camera, and extracting the edge points of the annular region as the annular edge point set; performing circle fitting on the annular edge point set after removing outliers using RANSAC to obtain the aperture center; selecting the point cloud within a preset depth range below the aperture center and fitting a cylindrical model to obtain the aperture axis direction.

[0062] In this embodiment: the structured light depth camera outputs a depth map and a point cloud, the point cloud being denoted as […] in the camera coordinate system. The homogeneous transformation from the camera to the end-hole assembly coordinate system is obtained through calibration. This achieves coordinate unification. Subsequently, the orifice ring area within a preset height range is extracted from the point cloud. The annular region is formed by a depth abrupt change zone near the aperture. The edge points of the annular region are extracted as the annular edge point set. To combat out-of-point interference caused by dust and reflection, RANSAC out-of-point removal is used before circle fitting. The minimum sampling set is determined by three points to define the circle, and candidate circle parameters are iteratively generated. and with residuals Determine the set of interior points, where To allow for the maximum distance error of a point deviating from the ideal circle, the candidate circle with the largest number of interior points is selected, and its interior points are refined using least-squares optimization to obtain the precise positioning of the orifice center. .

[0063] Furthermore, a point cloud is selected within a preset depth range below the center of the orifice. To fit the hole axis direction, this embodiment uses the axial estimation derivation of a cylindrical model: First, calculate the point cloud covariance matrix:

[0064]

[0065] Then, take the eigenvector corresponding to the largest eigenvalue as the unit vector of the hole axis direction. and with Together they constitute the pose description of the hole; from this, the fine deviation of the end effector relative to the hole assembly can be defined, where the planar fine deviation is taken as... The angle precision deviation is taken as ,here and These are the reference point and direction vector of the delivery tube axis, respectively; the output of the fine positioning model. and It will be used directly as the input for micro-motion control.

[0066] S7: When the precision deviation is greater than the second threshold, the control end micro-motion hole-setting mechanism performs a two-dimensional orthogonal translation in a plane perpendicular to the hole axis to compensate for the precision deviation; after each translation, S6 is repeated to update the precision deviation until the precision deviation is less than the second threshold and a hole-setting completion signal is output.

[0067] Specifically: The end-effector micro-motion orifice alignment mechanism includes an X-axis linear module and a Y-axis linear module arranged orthogonally to each other, as well as a movable platform whose displacement is controlled by the X-axis linear module and the Y-axis linear module. The maximum translational stroke of the X-axis linear module and the Y-axis linear module is 0.3 meters to 0.6 meters. The axis of the drug delivery tube is coaxially set with the movable platform of the end-effector micro-motion orifice alignment mechanism, so that the drug delivery tube can achieve equidistant translation with the two-dimensional translation of the movable platform.

[0068] In this embodiment, the end-effector micro-motion orifice alignment mechanism includes an X-axis linear module, a Y-axis linear module, and a movable platform arranged orthogonally to each other. The movable platform is driven by the superposition of the two linear modules to achieve two-dimensional orthogonal translation, and the maximum translation stroke in both the X and Y directions is 0.3 meters to 0.6 meters. The axis of the drug delivery tube is coaxially set with the translation center of the movable platform, so that the drug delivery tube can achieve equidistant translation with the two-dimensional translation of the movable platform. Based on this geometric relationship, the planar precision deviation can be directly mapped to the micro-motion compensation amount.

[0069] The compensation relationship is as follows: .

[0070] And through the stroke saturation function , The calculation is limited to the available travel of the movable platform to ensure that the compensation calculation is consistent with the mechanism's capabilities.

[0071] The controller executes a closed-loop iterative process, calculating the compensation amount, driving the linear module to its position, re-acquiring the structured light point cloud, and updating the fine deviation. In this embodiment, the second threshold is set to 0.01 meters, and the process is completed within the specified timeframe. <0.01 meters and When the angle is less than 3°, a hole alignment completion signal is output, providing clear and verifiable quantitative conditions for hole alignment determination and subsequent interlocking verification.

[0072] S8: After outputting the hole completion signal, trigger the insertion of the charging guide tube and the charging interlock condition verification; if the interlock condition is met, execute the charging operation; otherwise, enter the reversal process and trigger manual intervention prompt.

[0073] Specifically, the interlocking condition verification includes: the continuous stable time of the hole completion signal is greater than the third threshold; the hole opening area in the depth map of the structured light depth camera is unobstructed and meets the insertion determination; the angle between the insertion direction of the charging guide and the hole axis is less than the fourth threshold; the vehicle is in a parked state and the outriggers are in a supported position; the personnel detection result in the danger zone is that no personnel have entered; if any of the conditions are not met, charging is prohibited and the retraction process is initiated.

[0074] In this embodiment, the third threshold is set to 1 second to exclude instantaneous false detections; the depth map of the structured light depth camera shows that the orifice area is unobstructed and meets the insertion criteria, which is based on the depth continuity within the orifice area and the minimum through-hole diameter being greater than the outer diameter of the delivery tube; the angle between the insertion direction of the delivery tube and the orifice axis is less than the fourth threshold, which in this embodiment is set to 3 degrees and corresponds to the aforementioned angular precision deviation. The vehicle is parked and the outriggers are in a supported position to ensure the stability of the robotic arm during movement and insertion. The personnel detection result in the danger zone is that no personnel have entered to meet the safety regulations for blasting operations. If any of these conditions are not met, loading of explosives is prohibited and the process will begin in the retraction phase, while triggering a manual intervention prompt.

[0075] The rollback process restores the controllable state by returning the end-effector micro-motion platform to zero and re-executing the structured light precision hole alignment, thereby linking hole alignment success determination, insertion feasibility, and safety boundary verification into a complete traceable closed loop. Example 2:

[0076] An intelligent hole-finding and precise hole-aligning system for an on-site mixed explosives vehicle, employing any of the aforementioned hole-finding and precise hole-aligning methods for on-site mixed explosives vehicles, the system comprising:

[0077] GNSS RTK base station;

[0078] The first GNSS RTK mobile station is fixedly installed on the robotic arm base;

[0079] The second GNSS RTK mobile station is fixedly installed at the end of the robotic arm's hole-aligning assembly;

[0080] The robotic arm has its base fixedly connected to the vehicle body;

[0081] An end-mounted aperture assembly is fixedly connected to the end of a robotic arm. The end-mounted aperture assembly includes a laser scanning mechanism, a structured light depth camera, an end-mounted micro-motion aperture assembly mechanism, and a drug delivery tube mounting structure. The laser scanning mechanism and the structured light depth camera are jointly mounted on the movable platform of the end-mounted micro-motion aperture assembly mechanism. The drug delivery tube mounting structure is fixed on the movable platform, and the axis of the drug delivery tube is coaxial with the translation center of the movable platform.

[0082] The controller is communicatively connected to the first GNSS RTK mobile station, the second GNSS RTK mobile station, the robotic arm, the laser scanning mechanism, the structured light depth camera, and the end effector micro-motion hole-setting mechanism.

Claims

1. A method for hole searching and accurate hole aligning of a site mixed explosive car, characterized in that, The method steps are as follows: S1: Obtain the design coordinate set of each blast hole in the charging area, and establish a task queue; S2: Lay out a GNSS RTK reference station at the work site, and set a first mobile station and a second mobile station on the mixed explosive truck, wherein the first mobile station is installed at the base of the mechanical arm, and the second mobile station is installed at the end of the mechanical arm; S3: Solve the base coordinates of the base of the mechanical arm in the geographic coordinate system through the reference station and the first mobile station, solve the end coordinates of the end of the mechanical arm in the geographic coordinate system through the reference station and the second mobile station, and establish a base coordinate system of the mechanical arm with the base coordinates as the origin; S4: For the target blast hole, according to the design coordinates of the target blast hole, the base coordinate system of the mechanical arm and the kinematics model of the mechanical arm, the joint angle and the extension amount of the mechanical arm are solved by using the iterative inverse kinematics with constraints to make the end of the blast hole assembly reach the pre-alignment pose near the target blast hole; S5: Start the laser scanning mechanism installed on the end of the blast hole assembly to obtain the point cloud of the ground surface near the target blast hole, perform ground segmentation and outlier rejection on the point cloud, extract the candidate region of the hole in the remaining point cloud, and obtain the coarse positioning result of the hole center by circle fitting; calculate the coarse deviation of the end of the blast hole assembly relative to the hole center according to the coarse positioning result, control the mechanical arm to perform coarse alignment iteration until the coarse deviation is less than a first threshold value; S6: Start the structured light depth camera installed on the end of the blast hole assembly to obtain the depth map and point cloud of the hole of the target blast hole, convert the point cloud to the coordinate system of the end of the blast hole assembly, extract the annular edge point set in the hole region, and obtain the hole center by circle fitting with RANSAC outlier rejection, and fit a cylindrical model in the point cloud below the hole to obtain the hole axis direction, thereby obtaining the fine positioning result of the hole center and the hole axis direction; calculate the fine deviation of the end of the blast hole assembly relative to the hole center and the hole axis; S7: When the fine deviation is greater than a second threshold value, control the end of the blast hole assembly to perform two-dimensional orthogonal translation in the plane perpendicular to the hole axis to compensate for the fine deviation; after each translation, repeat S6 to update the fine deviation until the fine deviation is less than the second threshold value and output a hole alignment completion signal; S8: After outputting the hole alignment completion signal, trigger the insertion of the charging guide pipe and the charging interlocking condition verification; when the interlocking condition is met, perform the charging operation, otherwise, enter the rollback process and trigger the manual intervention prompt.

2. The method according to claim 1, characterized in that, In step S3: synchronize the observation data of the first mobile station and the second mobile station with time stamps and perform smoothing filtering, and determine the horizontal orientation of the base coordinate system by using the directional results of the dual-antenna installed at the base of the mechanical arm, thereby completing the pose transformation from the geographic coordinate system to the base coordinate system of the mechanical arm.

3. The method according to claim 1, characterized in that, In step S4, the constraint conditions for solving the joint angle and extension amount of the mechanical arm by iterative inverse kinematics are: mechanical arm joint angle limit constraint, mechanical arm extension stroke limit constraint, and minimum safety distance constraint between the end of the blast hole assembly and the vehicle body and the ground.

4. The method of claim 1, wherein, The step S5 of extracting the orifice candidate region and obtaining the coarse positioning result of the orifice center includes: searching a concave region satisfying the height mutation and the reduced point density in the remaining point cloud after removing the ground point cloud as the orifice candidate region, extracting a boundary point set of the orifice candidate region, and performing circle fitting on the boundary point set to obtain the orifice center coordinate.

5. The method of claim 1, wherein, The step S6 of extracting the annular edge point set and fitting the cylindrical model includes: intercepting an orifice ring belt region in a preset height range in the point cloud output by the structured light depth camera, extracting edge points of the ring belt region as the annular edge point set; performing circle fitting on the annular edge point set after removing the outliers by RANSAC to obtain the orifice center; and selecting the point cloud in a preset depth range below the orifice center and performing cylindrical model fitting to obtain the hole axis direction.

6. The method of claim 1, wherein: The end micro-motion hole aligning mechanism in step S7 includes an X-direction linear module, a Y-direction linear module arranged orthogonally to each other, and a movable platform controlled by the X-direction linear module and the Y-direction linear module to displace, the maximum displacement of the X-direction linear module and the Y-direction linear module is 0.3-0.6 meters, and the axis of the charging conduit is coaxially arranged with the movable platform of the end micro-motion hole aligning mechanism, so that the charging conduit is displaced equidistantly with the two-dimensional displacement of the movable platform.

7. The method of claim 1, wherein, The interlocking condition checking in step S8 includes: the continuous and stable time of the hole completion signal is greater than a third threshold value; the orifice region in the depth map of the structured light depth camera is not blocked and satisfies the insertable determination; the angle between the charging conduit insertion direction and the hole axis is less than a fourth threshold value; the vehicle is in the parking state and the support leg is in the supporting in-place state; the dangerous area personnel detection result is no personnel entering; and the charging is prohibited and the back-off process is entered when any of the conditions is not satisfied.

8. An intelligent hole searching and accurate hole aligning system of a site mixed explosive car, characterized in that, The on-site mixed explosive vehicle searching hole and accurate hole aligning method according to any one of claims 1-7, the system comprises: a GNSS RTK reference station; a first GNSS RTK mobile station fixedly installed at the base of the mechanical arm; a second GNSS RTK mobile station fixedly installed at the end hole aligning assembly of the mechanical arm; a mechanical arm, the base of which is fixedly connected to the vehicle body; an end hole aligning assembly fixedly connected to the end of the mechanical arm, the end hole aligning assembly comprising a laser scanning mechanism, a structured light depth camera, an end micro-motion hole aligning mechanism, and a charging conduit mounting structure, wherein the laser scanning mechanism and the structured light depth camera are jointly installed on the movable platform of the end micro-motion hole aligning mechanism, the charging conduit mounting structure is fixed on the movable platform and the axis of the charging conduit is coaxial with the displacement center of the movable platform; a controller in communication connection with the first GNSS RTK mobile station, the second GNSS RTK mobile station, the mechanical arm, the laser scanning mechanism, the structured light depth camera, and the end micro-motion hole aligning mechanism.

Images