Pose real-time measurement system and method for mining excavation robot working device

Abstract

The invention discloses a multi-sensor fusion-based position and posture estimation method for a working device of a mining robot, which realizes accurate position and posture estimation by installing a body posture sensor and an environment sensing sensor and combining a kinematic model, visual inertia state estimation and a nonlinear least square optimization algorithm. The method comprises the following steps: firstly, carrying out preliminary prediction on a kinematic model through feedback data of an encoder and an inclinometer, and constructing a local visual inertia estimator by utilizing a camera and an MEMS to correct a prediction result; and then, a combined constraint modeling and optimization method is adopted to improve the precision of the pose of each working mechanism. Besides, geometric features of the bucket are extracted through a semantic segmentation network and a template matching technology, six-degree-of-freedom pose estimation of the bucket is completed, finally data fusion of a body pose sensor and an environment perception sensor is performed through a factor graph optimization algorithm, and comprehensive pose estimation of the whole working device is achieved. The working pose of the mining robot can be monitored in real time, the operation precision is improved, human errors are reduced, the working efficiency and safety can be optimized through data analysis, and more effective support is provided for path planning of a working device of the mining robot.

Description

Technical Field

[0001] The present application relates to the field of engineering machinery, and in particular to a method for real-time measurement of the position and posture of a working device of a mining excavation robot. Background Art

[0002] At present, there is little targeted work on the estimation of working mechanisms and target poses of engineering machinery using deep learning methods, and most of them are in the theoretical research stage. The more advanced target recognition and pose estimation method in academia is 6D pose estimation based on deep learning, which obtains the three-degree-of-freedom translation information and three-degree-of-freedom rotation information of the object to be measured in the camera coordinate system. According to whether there is a 3D point cloud model of the object to be measured, it is divided into instance-level and category-level pose estimation. For objects with rich textures, visual color images are used to construct 2D image and 3D model matching pairs through feature extraction, and the PnP method is used to restore the 6DPose of the object. For objects with weak textures or no texture, template-based methods or machine learning-based voting methods can be used to obtain the optimal 6D pose.

[0003] Typical solutions for 6D pose estimation based on point clouds are FrustrumPointNets and VoxelNet, which have achieved state-of-the-art performance on the Kitti benchmark. Unlike urban driving applications that can provide sufficient information using only point clouds, general object pose estimation tasks require reasoning about geometric and appearance information. RGB-D images fused with laser and vision have better application prospects. Posecnn estimates the 3D translation of an object by locating its center in the image and predicting its distance from the camera; and estimates the 3D rotation of the object through the regressed quaternion representation. Densefusion predicts the 6D pose of an object by fusing color features and depth features pixel by pixel. RGB-D images can reflect the appearance information of the estimated object and restore geometric information through camera intrinsic parameters to obtain more accurate and robust six-degree-of-freedom pose estimation information. China University of Mining and Technology proposed a method for 6D pose estimation of a target using RGB-D color point clouds fused with laser and visual calibration, which was applied to the identification and positioning of the tunnel end of an underground robot, and obtained an accurate complete estimation of the robot's target position and pose. Summary of the invention

[0004] The invention discloses a method for estimating the posture of an excavator working device based on multiple sensors. A body posture sensor and an environment perception sensor are installed on each working mechanism, and a computing unit is used to process sensor data.

[0005] Body posture sensors and environmental perception sensors are installed on each working mechanism of the mining excavator robot, and the sensor data is processed using a computing unit.

[0006] The body attitude sensor is used to measure and monitor the internal state information of the working device itself, such as motion, attitude, acceleration, angular velocity, etc., and is used for the work mechanism posture estimation based on kinematics. The environmental perception sensor senses and identifies the position or attitude of the working mechanism in space through the onboard sensor, and is used for the work mechanism 6dof posture estimation based on active observation.

[0007] Among them, three encoders are respectively installed at the connection between the boom and the body, the connection between the boom and the arm, and the connection between the arm and the bucket, which are used to measure the rotation angle and speed and feedback the movement of the working device; three inclinometers are respectively installed on the sides of the boom, arm, and bucket lifting ears to measure the inclination angle of the working device and provide additional angle information; three MEMS inertial measurement units and three industrial cameras are respectively installed on the sides of the boom, arm, and bucket to be used as visual odometers to capture image data and provide visual information for tracking and identifying the position and posture of the working mechanism; a multi-line laser radar is installed above the body to collect three-dimensional point cloud data during the movement of the working mechanism; a depth camera is installed above the cab to collect RGB images and depth value data during the movement of the working mechanism.

[0008] Furthermore, the computing unit comprises:

[0009] Industrial computer: equipped with high-performance processor and Linux operating system;

[0010] GPU accelerator: for deep learning and image processing tasks;

[0011] Memory and storage devices: used to store and manage sensor data and calculation results.

[0012] A method for real-time measurement of the position and posture of a mining robot working device, characterized in that it comprises the following steps:

[0013] Step S1, estimating the joint posture of a mining robot based on proprioceptors.

[0014] Step S2: identifying and estimating the position and posture of the mining robot bucket based on the airborne sensing sensor;

[0015] Step S3, performing joint pose estimation on the mining robot working device based on the above two steps;

[0016] Furthermore, the method for real-time measurement of the posture of a mining excavation robot working device is characterized in that the estimation of the joint posture of the mining excavation robot based on the proprioceptors in step S1 includes the following steps:

[0017] Step S11: Construct a kinematic model of the excavator working device according to the three-dimensional model, the constraint relationship and driving mode of the working mechanism. Use the three-dimensional geometric model of the excavator working device to determine the position and motion mode of each working mechanism; consider the mechanical connection and constraints between the working mechanisms, such as the motion range of the joints and connecting rods; and use the geometric parameters (such as connecting rod length, joint type) and kinematic principles of the mining robot to establish a forward kinematic model.

[0018] Step S12: Use the measurement feedback from the encoder and the inclinometer to make a preliminary prediction of the kinematic model. Input the data provided by the encoder and the inclinometer into the model, and use these data to calculate the position, posture and motion state of each part of the current working device.

[0019] Step S13: Combine the camera and the MEMS inertial measurement unit to construct a local visual inertial state estimator to estimate the motion state of each working mechanism of the working device, and perform observation correction on the prediction results of the kinematic model;

[0020] Step S14: perform joint constraint modeling on the pose estimation results of each working mechanism, and construct a nonlinear least squares joint optimization function to obtain a joint optimization state estimation of the mechanism under hard constraint conditions.

[0021] Furthermore, the real-time measurement method of the position and posture of a working device of a mining excavator robot is characterized in that, in step S13, a local visual inertial state estimator is constructed by combining a camera and a MEMS inertial measurement unit to estimate the motion state of each working mechanism of the working device, and the prediction result of the kinematic model is observed and corrected, including the following steps:

[0022] Step S131, the camera extracts feature points of the soil, rocks and surrounding obstacles in the operating environment of the mining robot through the feature point extraction algorithm SIFT (detects and describes local feature points in the image, and is invariant to scale and rotation), describes and matches the extracted feature points, maintains the association between consecutive frames during the movement of the excavator, obtains the structural information and relative motion of the workspace, and infers its position and posture in space.

[0023] Step S132, the inertial measurement unit (IMU) obtains acceleration information and angular velocity information; wherein the boom, dipper arm, and bucket are each equipped with a MEMS inertial measurement unit and an industrial camera. In this arrangement, the IMU of each component can independently obtain its specific motion state, reducing the error caused by overall movement; each camera provides environmental information from different perspectives, enhancing the perception of the surrounding environment, especially in complex and dynamic construction scenes; it can significantly improve the accuracy of the motion state estimation of the working device of the mining excavator robot, the environmental perception ability and the robustness,

[0024] Step S133, using an extended Kalman filter (EKF) method to fuse the information obtained by the camera and the information obtained by the IMU in a recursive filtering framework to estimate the state of the working device;

[0025] Step S134: Correct the result of visual-inertial state estimation and the prediction result of the kinematic model. In this way, the deviation in the model prediction is corrected and the accuracy of state estimation is improved.

[0026] Furthermore, the real-time measurement method for the posture of a mining robot working device is characterized in that, in step S14, joint constraint modeling is performed on the posture estimation results of each working mechanism, and a nonlinear least squares joint optimization function is constructed to obtain a joint optimization state estimation of the mechanism under hard constraint conditions, including:

[0027] The purpose of joint constraint modeling is to integrate the estimated results of the motion states of the boom, arm, and bucket to ensure the kinematic and dynamic consistency of the overall system;

[0028] First, we need to consider the constraints, including kinematic constraints, dynamic constraints, and hard constraints (link length remains constant, joint range of motion is limited). The pose estimation results of each mechanism are used as constraints for modeling; the Gauss-Newton method is used to construct the optimization function:

[0029]

[0030] in, Indicates The residuals of the constraints.

[0031] Update the state estimation of the working mechanism according to the optimization results, including position, posture, speed, etc.

[0032] Furthermore, the method for real-time measurement of the position and posture of a mining robot working device is characterized in that the mining robot bucket is identified and estimated based on the environment perception sensor in step S2, comprising the following steps:

[0033] Step S21, calibrate the internal and external parameters of the depth camera and the laser radar. The camera internal parameter calibration equation is:

[0034] [ u v ] = 1 Z [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ]

[0035] in, are image coordinates, is the world coordinate, and is the focal length, is the principal point location;

[0036] By aligning the depth camera with the point cloud and image of the LiDAR, the relative position and attitude between the two are calculated, and the external parameter calibration designs the transformation relationship between the camera and the LiDAR, which usually includes the rotation matrix R and the translation vector t. The calibration equation can be expressed as:

[0037] [ X li dar Y lidar Z lidar ] = R [ X camera Y camera Z camera ] + t

[0038] in, are the LiDAR coordinates, are the camera coordinates;

[0039] Step S22: constructing a point cloud template and a depth image template of the excavator bucket based on the multi-line laser radar and depth camera data collected offline;

[0040] Step S23: By collecting projections of the bucket 3D model from different viewing angles and combining it with the bucket CAD model, a template library of bucket 2D images and 6D poses is constructed. The 3D model of the bucket is exported from the CAD system, and the 6D pose (position and posture) of the model is defined; the 3D model of the bucket is projected from different viewing angles to generate 2D images, ensuring that each 2D image corresponds to a unique 6D pose; the 2D images from different viewing angles are combined with their corresponding 6D poses to establish a template library of images and poses;

[0041] Step S24: using radar and camera to collect point clouds and images of the bucket at different viewing angles, and manually annotating the geometric features of the bucket, and then training a semantic segmentation network model;

[0042] Step S25, obtain the bucket template through the offline trained 3D point cloud and 2D image model for template matching and positioning. In practical applications, use a depth camera to collect the 2D image of the bucket, use a multi-line laser radar to obtain the 3D point cloud data of the bucket, use the offline trained semantic segmentation network to process the current collected image, and extract the geometric features of the bucket; match the extracted bucket image features with the images in the template library, and use the mean square error to calculate the matching degree between the current image and each template in the template library; complete the six-degree-of-freedom pose estimation of the bucket through the pose information obtained by matching;

[0043] The mean square error is used to measure the difference between the current image and the template image. The calculation method is to take the average of the square of the error of each pixel. The formula is as follows:

[0044]

[0045] in, The current image and the template image are The gray value of is the total number of pixels in the image.

[0046] Step S26, in the online stage, after the end camera of the excavator collects the 2D image of the bucket, the bucket image under the current perspective is extracted through semantic segmentation and matched with the image in the template library. All modules are integrated into the control system of the excavator for comprehensive testing; check whether the overall process of data acquisition, semantic segmentation, template matching and pose estimation is smooth; according to the test results, adjust and optimize the system parameters, deploy the system on the excavator, and perform on-site operations.

[0047] Furthermore, the method for real-time measurement of the position and posture of a mining robot working device is characterized in that, in step S22, a point cloud template and a depth image template of the excavator bucket are constructed based on the multi-line laser radar and depth camera data collected offline, comprising the following steps:

[0048] Step S221, using a multi-line laser radar to obtain point cloud data of the bucket, covering different viewing angles of the bucket;

[0049] Step S222: filtering, denoising, registering, etc. the point cloud data to obtain a clear bucket model, integrating and aligning the processed point cloud data to generate a three-dimensional point cloud template of the bucket;

[0050] Step S223: Use the depth camera to obtain depth image data of the bucket, also covering different viewing angles, correct the depth image data, eliminate camera distortion, and generate a depth image template of the bucket from the depth images at different viewing angles.

[0051] Furthermore, the real-time measurement method for the position and posture of a mining robot working device is characterized in that, in step S24, radar and camera are used to collect point clouds and images of the bucket at different perspectives, and the geometric features of the bucket are manually annotated to train the semantic segmentation network model; the following steps are included: manually annotating the collected point cloud data and images, marking the geometric feature area of ​​the bucket, for example, marking the edge and corner points of the bucket in the point cloud, and marking the outline of the bucket in the image; converting the annotated point cloud data and images into a format suitable for training, such as input point cloud features and corresponding labels; semantic segmentation network architecture, offline training of the annotated data, and optimization of network parameters to improve the model's segmentation accuracy for the bucket's geometric features; finally, using an independent validation set for testing, evaluating the performance of the model, and making necessary adjustments and optimizations.

[0052] Assume the input image is , the semantic segmentation output is ,in is a binary image representing the bucket area.

[0053]

[0054] Extract feature points from the results of semantic segmentation To perform pose estimation, a convolutional neural network (CNN) is used for feature extraction:

[0055]

[0056] Based on the extracted features, a pose estimation network is constructed , output the six-degree-of-freedom position of the bucket (Location( ) and rotation ( )):

[0057] Pose Estimation Network :

[0058]

[0059]

[0060] The estimated pose is unified into a global coordinate system, and the following coordinate system transformation is assumed: The transformation matrix from the local coordinate system (bucket coordinate system) to the world coordinate system is , which contains rotation and translation information.

[0061] Conversion formula:

[0062]

[0063] Integrating the above steps, we get the final bucket pose estimation formula:

[0064]

[0065] Furthermore, the real-time measurement method of the posture of a mining excavation robot working device is characterized in that the combined step S1 and step S2 estimate the posture of the mining excavation robot working device, including: integrating the results of step S1 and step S2, fusing the joint posture information obtained by the body posture sensor and the bucket posture information provided by the environmental perception sensor through a factor graph optimization algorithm, and performing comprehensive posture estimation of the mining excavation robot working device.

[0066] Nodes of the factor graph: The working device of the mining robot has three joints, and the pose information of the joints and bucket position information ;

[0067] Factors (constraints) include entity sensor factors and environmental sensor factors.

[0068] Ontological sensor factor:

[0069] The encoder constraint factor is defined as

[0070]

[0071] in, It is The measurement values ​​obtained from the encoder for each joint, is the true angle of the joint calculated by the kinematic model;

[0072] The inclinometer constraint factor is defined as:

[0073]

[0074] in Excavator working device Measurements obtained from the inclinometer for each joint;

[0075] The Imu constraint factor is defined as:

[0076]

[0077] in, It is the current moment The location of the working device, It was the moment before location, It was the moment before The velocity is obtained by integrating the acceleration and time, is the acceleration at the previous moment, It is the time interval between the current moment and the previous moment.

[0078] The camera constraint factor is defined as:

[0079]

[0080] in, It is a two-dimensional point measured by a camera installed on the working device. is the camera’s intrinsic parameter matrix, is the homogeneous transformation matrix of the camera's pose, are camera feature points.

[0081] Environmental sensor factors:

[0082] LiDAR Constraint Factors:

[0083]

[0084] in, Represents the transformation from the world coordinate system to the lidar coordinate system, Indicates the current position of the mining robot. Indicates the position of the model in the robot coordinate system (such as a geometric feature point of the bucket), It is the predicted position of the lidar feature point in the world coordinate system calculated by the current state estimation.

[0085] Depth camera constraint factor:

[0086]

[0087] in, represents the pose of the depth camera, represents the depth value of the 3D point projected from the bucket position to the image plane, Indicates the depth value obtained by the depth camera.

[0088]

[0089] is the camera’s intrinsic parameter matrix, and are the rotation matrix and displacement vector of the camera respectively.

[0090] The final optimization objective function is:

[0091]

[0092] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention.

[0094] Figure 1 Schematic diagram of the installation position of the encoder of the present invention

[0095] Figure 2 Schematic diagram of the installation position of the inclinometer of the present invention

[0096] Figure 3 Schematic diagram of the installation position of the MEMS inertial measurement unit and the industrial camera of the present invention

[0097] Figure 4 Schematic diagram of the installation position of the laser radar of the present invention

[0098] Figure 5 Schematic diagram of the installation position of the depth camera of the present invention

[0099] Figure 6 Schematic diagram of the workflow of the present invention

[0100] Reference numerals

[0101] 1-1, Boom encoder installation; 1-2, Arm encoder installation; 1-3, Bucket encoder installation; 2-1, Boom inclinometer installation; 2-2, Arm inclinometer installation; 2-3, Bucket inclinometer installation; 3-1, Boom MEMS inertial measurement unit and industrial camera installation; 3-2, Arm MEMS inertial measurement unit and industrial camera installation; 3-3, Bucket MEMS inertial measurement unit and industrial camera installation; DETAILED DESCRIPTION

[0102] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. The components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations. The specific model specifications need to be selected and determined according to the actual specifications of the device, and the specific selection calculation method adopts the existing technology in the field, so it will not be described in detail.

[0103] Example

[0104] This embodiment combines Figures 1 to 6 The method for estimating the posture of an excavator working device based on multiple sensors according to the present invention is described as follows:

[0105] (1) Kinematic model construction

[0106] An encoder is installed at the connection between the boom and the body, the connection between the boom and the arm, and the connection between the arm and the bucket; an inclinometer is installed on the side of the boom, the arm and the bucket; a MEMS inertial measurement unit and a camera are installed on the side of the boom, the arm and the bucket; a multi-line laser radar is installed above the excavator control room; and a depth camera is installed at the end of the excavator.

[0107] Use the three-dimensional geometric model of the excavator working device to determine the position and movement of each working mechanism; consider the mechanical connections and constraints between the working mechanisms, such as the motion range of joints and connecting rods; use the geometric parameters of the mining excavation robot (such as connecting rod length, joint type) and kinematic principles to establish forward and inverse kinematic models.

[0108] Calibration of intrinsic and extrinsic parameters of depth camera and lidar.

[0109] S1, using a chessboard as a calibration plate;

[0110] S2, collect data, collect the point cloud data of the laser radar, obtain image data through the camera, and ensure that the image and laser radar data are in the same scene;

[0111] S3, using the corner detection algorithm to detect the corners of each chessboard on the calibration board, calculate the camera internal parameters according to their image coordinates, and complete the calibration of the internal parameters. The camera internal parameter calibration equation is:

[0112] [ u v ] = 1 Z [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ]

[0113] in, are image coordinates, is the world coordinate, and is the focal length, is the principal point location;

[0114] S4. By aligning the point cloud and image of the camera with the lidar, the relative position and attitude between the two are calculated, and the external parameter calibration designs the transformation relationship between the camera and the lidar, which usually includes the rotation matrix R and the translation vector t. The calibration equation can be expressed as:

[0115] [ X li dar Y lidar Z lidar ] = R [ X camera Y camera Z camera ] + t

[0116] in, are the LiDAR coordinates, are the camera coordinates;

[0117] (2) Preliminary forecast

[0118] The data provided by the encoder and inclinometer are input into the model, and the data is used to calculate the position, posture and motion state of each part of the current working device. For example, the angle data of the encoder is used to update the joint position of the working mechanism, and the data of the inclinometer is used to adjust the posture of the entire device.

[0119] (3) Local visual-inertial state estimation

[0120] The camera extracts the feature points of the current environment through the feature point extraction algorithm SIFT (detects and describes local feature points in the image, and is invariant to scale and rotation).

[0121] S1. Scale space construction. The scale space is generated by Gaussian blurring the image. Gaussian blurring can be achieved using the following formula:

[0122]

[0123] in, is the Gaussian kernel function, are image coordinates, is the standard deviation of the Gaussian blur;

[0124] The image is blurred step by step to generate images of multiple scales; through different standard deviations Generates a Gaussian pyramid of an image.

[0125] S2. Key point detection. In scale space, key point detection depends on finding local extreme points. Assume is an image in scale space. By comparing the image with its neighborhood, local extrema can be detected. Local extrema of key points:

[0126]

[0127] S3, key point positioning, Hessian matrix is ​​used to evaluate the stability of key points:

[0128] H = [ D xx D xy D xy D yy ]

[0129] in, , , is the matrix of the second-order derivatives of the image.

[0130] Stability of key points:

[0131]

[0132]

[0133] Removing keypoints with low contrast and strong edge response is usually done by calculating their principal curvature and edge response.

[0134] S4, direction assignment, calculate the gradient amplitude of each pixel and direction :

[0135]

[0136]

[0137] in, and The image is Directional gradient.

[0138] Generate a histogram of directions in the neighborhood of the keypoint and select the dominant direction:

[0139] H ( θ ) = ∑ i ∈ cell m i for θ i in [ θ − 15 ∘ , θ + 15 ∘ ]

[0140] S5. In the neighborhood around the main direction of the key point, the gradient is calculated and quantized into a 128-dimensional feature descriptor; the gradient direction histogram in the neighborhood is calculated, divided into 4x4 grids, and each grid generates a histogram of 8 directions, a total of 128 dimensions:

[0141]

[0142] Then describe these points and find the same points in consecutive image frames or different viewing angles to match feature points, obtain the structural information and relative motion of the environment, and infer its position and posture in space; the inertial measurement unit (IMU) obtains acceleration information and angular velocity information;

[0143] The extended Kalman filter (EKF) method is used to fuse the information obtained by the camera and the information obtained by the IMU in a recursive filtering framework to estimate the state of the working device:

[0144] S1. Define the system model and observation model. First, establish the state model of the working device. Assume that the state of the working device is determined by the position and speed The state equation can be expressed as:

[0145]

[0146] in, is a nonlinear function of the system, is the control input (acceleration and angular velocity provided by the IMU), is the process noise, which is assumed to be Gaussian distributed.

[0147] The camera provides information about the position of the working device or other relevant features. The observation equation can be expressed as:

[0148]

[0149] in, is the observation model, is the observation data (camera measurement result), is the observation noise, which is assumed to also conform to the Gaussian distribution.

[0150] S2, initialize the state estimate and covariance matrix, initial state , the initial covariance matrix

[0151] S3. Use the state transition model to predict the state and covariance at the next moment:

[0152] Status prediction:

[0153]

[0154] Covariance prediction:

[0155]

[0156] in, is the Jacobian matrix of the state transition model, is the process noise covariance matrix.

[0157] S4. Fuse the camera’s observation data with the predicted state and calculate the Kalman gain:

[0158]

[0159] in, is the Jacobian matrix of the observation model, is the observation noise covariance matrix.

[0160] Update the state estimate:

[0161]

[0162] Update the covariance matrix:

[0163]

[0164] S5. Use IMU data (acceleration and angular velocity) as control input To update the status:

[0165] IMU data usually provides acceleration and angular velocity ; In the state prediction stage, This data can be used directly to update the state of the working device. The results of the visual inertial state estimation are corrected with the prediction results of the kinematic model. In this way, the deviation in the model prediction is corrected and the accuracy of the state estimation is improved.

[0166] (4) Joint Optimization

[0167] Considering the constraint relationship between the working mechanisms, the pose estimation results of each mechanism are modeled as constraints; an optimization function is established to combine the prediction results and the actual measurement data, and perform minimization processing to obtain the optimal joint state estimation.

[0168] (5) Constructing point cloud template and depth image template

[0169] Use a multi-line laser radar to obtain the point cloud data of the bucket, covering different perspectives of the bucket, and perform filtering, denoising, and registration on the point cloud data to obtain a clear bucket model. The processed point cloud data are integrated and aligned to generate a three-dimensional point cloud template of the bucket. Use a depth camera to obtain the depth image data of the bucket, also covering different perspectives. The depth image data is corrected to eliminate camera distortion, and a depth image template of the bucket is generated from depth images at different perspectives.

[0170] (6) Build a template library corresponding to the bucket 2D image and 6D pose

[0171] Export the 3D model of the bucket from the CAD system and define the 6D pose (position and attitude) of the model; project the 3D model of the bucket from different perspectives to generate 2D images, ensure that each 2D image corresponds to a unique 6D pose, combine the 2D images from different perspectives with their corresponding 6D poses, and establish a template library of images and poses.

[0172] (7) Manually annotate the collected point cloud data and images, and mark the geometric feature areas of the bucket. For example, mark the edges and corners of the bucket in the point cloud, and mark the outline of the bucket in the image; convert the annotated point cloud data and images into a format suitable for training, such as the input point cloud features and corresponding labels; use the U-Net or DeepLab semantic segmentation network architecture to perform offline training on the annotated data and optimize the network parameters to improve the model's segmentation accuracy for the bucket's geometric features; finally, use an independent validation set for testing to evaluate the performance of the model and make necessary adjustments and optimizations.

[0173] (8) Manually label geometric features and train semantic segmentation network models

[0174] Manually annotate the collected point cloud data and images, mark the geometric feature area of ​​the bucket, for example, mark the edge and corner points of the bucket in the point cloud, and mark the outline of the bucket in the image; convert the annotated point cloud data and images into a format suitable for training, such as the input point cloud features and corresponding labels; use U-Net or DeepLab semantic segmentation network architecture to perform offline training on the annotated data and optimize network parameters to improve the model's segmentation accuracy for the bucket's geometric features; S1, convolution operation

[0175]

[0176] in, is the input image, is the convolution kernel, is the position coordinate;

[0177] S2. Pooling operation

[0178] Max Pooling selects the maximum value of a local area. The formula is:

[0179]

[0180] in, are the position coordinates within the pooling window;

[0181] S3, upsampling in decoder:

[0182] Upsampling enlarges the feature map by interpolation. The formula can be expressed as:

[0183]

[0184] in, is the size of the pooling window;

[0185] S4, skip connection:

[0186] Connect the feature map of the encoder stage with the feature map of the decoder stage, the formula is:

[0187]

[0188] in, is the encoder output, is the decoder output;

[0189] S5. Loss function:

[0190]

[0191] Finally, an independent validation set is used for testing to evaluate the performance of the model and make necessary adjustments and optimizations.

[0192] (9) Using offline training models for template matching and positioning

[0193] In practical applications, a depth camera is used to collect 2D images of the bucket, a multi-line lidar is used to obtain 3D point cloud data of the bucket, and an offline trained semantic segmentation network is used to process the currently collected images to extract the geometric features of the bucket. The extracted bucket image features are matched with the images in the template library, and the similarity measurement method is used to calculate the matching degree between the current image and each template in the template library. The six-degree-of-freedom pose estimation of the bucket is completed through the pose information obtained by matching.

[0194] (10) Online stage: 2D image semantic segmentation and pose estimation

[0195] In the online stage, after the camera at the end of the excavator collects the 2D image of the bucket, it extracts the bucket image from the current perspective through semantic segmentation and matches it with the image in the template library, including: integrating all modules into the excavator's control system for comprehensive testing; checking whether the overall process of data acquisition, semantic segmentation, template matching and pose estimation is smooth; adjusting and optimizing system parameters based on the test results, deploying the system on the excavator for on-site operation.

[0196] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A real-time position measurement system for a mining excavation robot working device, characterized in that: include: Body posture sensors and environmental perception sensors are installed on each working mechanism of the mining excavator robot, and a computing unit is used to process the sensor data. The body attitude sensor is used to measure and monitor the internal state information of the working device itself, such as motion, attitude, acceleration, angular velocity, etc., and is used for the work mechanism posture estimation based on kinematics. The environmental perception sensor senses and identifies the position or attitude of the working mechanism in space through the onboard sensor, and is used for the work mechanism 6dof posture estimation based on active observation. Among them, three encoders are respectively installed at the connection between the boom and the body, the connection between the boom and the arm, and the connection between the arm and the bucket, which are used to measure the rotation angle and speed and feedback the movement of the working device; three inclinometers are respectively installed on the sides of the boom, arm, and bucket lifting ears to measure the inclination angle of the working device and provide additional angle information; three MEMS inertial measurement units and three industrial cameras are respectively installed on the sides of the boom, arm, and bucket to be used as visual odometers to capture image data and provide visual information for tracking and identifying the position and posture of the working mechanism; a multi-line laser radar is installed above the body to collect three-dimensional point cloud data during the movement of the working mechanism; a depth camera is installed above the cab to collect RGB images and depth value data during the movement of the working mechanism. The computing unit comprises: Industrial computer: equipped with high-performance processor and Linux operating system; GPU accelerator: for deep learning and image processing tasks; Memory and storage devices: used to store and manage sensor data and calculation results.

2. A method for real-time measurement of the position and posture of a mining robot working device, characterized in that: The following steps are included: Step S1, estimating the joint posture of a mining robot based on proprioceptors. Step S2: identifying and estimating the position and posture of the mining robot bucket based on the airborne sensing sensor; Step S3: Based on the above two steps, a joint pose estimation is performed on the mining excavation robot working device.

3. A method for real-time measurement of the position and posture of a mining robot working device according to claim 2, characterized in that: In step S1, the joint posture estimation of the mining robot based on the proprioceptors includes the following steps: Step S11: Construct a kinematic model of the excavator working device according to the three-dimensional model, the constraint relationship and driving mode of the working mechanism. Use the three-dimensional geometric model of the excavator working device to determine the position and motion mode of each working mechanism; consider the mechanical connection and constraints between the working mechanisms, such as the motion range of the joints and connecting rods; and use the geometric parameters (such as connecting rod length, joint type) and kinematic principles of the mining robot to establish a forward kinematic model. Step S12: Use the measurement feedback from the encoder and the inclinometer to make a preliminary prediction of the kinematic model. Input the data provided by the encoder and the inclinometer into the model, and use these data to calculate the position, posture and motion state of each part of the current working device. Step S13: Combine the camera and the MEMS inertial measurement unit to construct a local visual inertial state estimator to estimate the motion state of each working mechanism of the working device, and perform observation correction on the prediction results of the kinematic model; Step S14: perform joint constraint modeling on the pose estimation results of each working mechanism, and construct a nonlinear least squares joint optimization function to obtain a joint optimization state estimation of the mechanism under hard constraint conditions.

4. A method for real-time measurement of the position and posture of a mining robot working device according to claim 3, characterized in that: In step S13, a local visual inertial state estimator is constructed by combining the camera and the MEMS inertial measurement unit to estimate the motion state of each working mechanism of the working device, and the prediction result of the kinematic model is observed and corrected, including the following steps: Step S131, the camera extracts feature points of the soil, rocks and surrounding obstacles in the operating environment of the mining robot through the feature point extraction algorithm SIFT (detects and describes local feature points in the image, and is invariant to scale and rotation), describes and matches the extracted feature points, maintains the association between consecutive frames during the movement of the excavator, obtains the structural information and relative motion of the workspace, and infers its position and posture in space. Step S132, the inertial measurement unit (IMU) obtains acceleration information and angular velocity information; wherein the boom, dipper arm, and bucket are each equipped with a MEMS inertial measurement unit and an industrial camera. In this arrangement, the IMU of each component can independently obtain its specific motion state, reducing the error caused by overall movement; each camera provides environmental information from different perspectives, enhancing the perception of the surrounding environment, especially in complex and dynamic construction scenes; it can significantly improve the accuracy of the motion state estimation of the working device of the mining excavator robot, the environmental perception ability and the robustness, Step S133, using an extended Kalman filter (EKF) method to fuse the information obtained by the camera and the information obtained by the IMU in a recursive filtering framework to estimate the state of the working device; Step S134: Correct the result of visual-inertial state estimation and the prediction result of the kinematic model. In this way, the deviation in the model prediction is corrected and the accuracy of state estimation is improved.

5. The method for real-time measurement of the position and posture of a mining robot working device according to claim 3, characterized in that: In step S14, joint constraint modeling is performed on the pose estimation results of each working mechanism, and a nonlinear least squares joint optimization function is constructed to obtain a joint optimization state estimation of the mechanism under hard constraint conditions, including: The purpose of joint constraint modeling is to integrate the estimated results of the motion states of the boom, arm, and bucket to ensure the kinematic and dynamic consistency of the overall system; First, we need to consider the constraints, including kinematic constraints, dynamic constraints, and hard constraints (link length remains constant, joint range of motion is limited). The pose estimation results of each mechanism are used as constraints for modeling; the Gauss-Newton method is used to construct the optimization function: in, Indicates The residuals of the constraints. Update the state estimation of the working mechanism according to the optimization results, including position, posture, speed, etc.

6. A method for real-time measurement of the position and posture of a mining robot working device according to claim 2, characterized in that: In step S2, the bucket of the mining robot is identified and the posture is estimated based on the environment perception sensor, including the following steps: Step S21, calibrate the internal and external parameters of the depth camera and the laser radar. The camera internal parameter calibration equation is: [ u v ] = 1 Z [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] in, are image coordinates, is the world coordinate, and is the focal length, is the principal point location; By aligning the depth camera with the point cloud and image of the LiDAR, the relative position and attitude between the two are calculated, and the external parameter calibration designs the transformation relationship between the camera and the LiDAR, which usually includes the rotation matrix R and the translation vector t. The calibration equation can be expressed as: [ X li dar Y lidar Z lidar ] = R [ X camera Y camera Z camera ] + t in, are the LiDAR coordinates, are the camera coordinates; Step S22: constructing a point cloud template and a depth image template of the excavator bucket based on the multi-line laser radar and depth camera data collected offline; Step S23: By collecting projections of the bucket 3D model from different viewing angles and combining it with the bucket CAD model, a template library of bucket 2D images and 6D poses is constructed. The 3D model of the bucket is exported from the CAD system, and the 6D pose (position and posture) of the model is defined; the 3D model of the bucket is projected from different viewing angles to generate 2D images, ensuring that each 2D image corresponds to a unique 6D pose; the 2D images from different viewing angles are combined with their corresponding 6D poses to establish a template library of images and poses; Step S24: using radar and camera to collect point clouds and images of the bucket at different viewing angles, and manually annotating the geometric features of the bucket, and then training a semantic segmentation network model; Step S25, obtain the bucket template through the offline trained 3D point cloud and 2D image model for template matching and positioning. In practical applications, use a depth camera to collect the 2D image of the bucket, use a multi-line laser radar to obtain the 3D point cloud data of the bucket, use the offline trained semantic segmentation network to process the current collected image, and extract the geometric features of the bucket; match the extracted bucket image features with the images in the template library, and use the mean square error to calculate the matching degree between the current image and each template in the template library; complete the six-degree-of-freedom pose estimation of the bucket through the pose information obtained by matching; The mean square error is used to measure the difference between the current image and the template image. The calculation method is to take the average of the square of the error of each pixel. The formula is as follows: in, The current image and the template image are The gray value of is the total number of pixels in the image. Step S26, in the online stage, after the end camera of the excavator collects the 2D image of the bucket, the bucket image under the current perspective is extracted through semantic segmentation and matched with the image in the template library. All modules are integrated into the control system of the excavator for comprehensive testing; check whether the overall process of data acquisition, semantic segmentation, template matching and pose estimation is smooth; according to the test results, adjust and optimize the system parameters, deploy the system on the excavator, and perform on-site operations.

7. A method for real-time measurement of the position and posture of a mining robot working device according to claim 6, characterized in that: In step S22, based on the multi-line laser radar and depth camera data collected offline, a point cloud template and a depth image template of the excavator bucket are constructed, including the following steps: Step S221, using a multi-line laser radar to obtain point cloud data of the bucket, covering different viewing angles of the bucket; Step S222: filtering, denoising, registering, etc. the point cloud data to obtain a clear bucket model, integrating and aligning the processed point cloud data to generate a three-dimensional point cloud template of the bucket; Step S223: Use the depth camera to obtain depth image data of the bucket, also covering different viewing angles, correct the depth image data, eliminate camera distortion, and generate a depth image template of the bucket from the depth images at different viewing angles.

8. A method for real-time measurement of the position and posture of a mining robot working device according to claim 6, characterized in that: In step S24, radar and camera are used to collect point clouds and images of the bucket at different viewing angles, and the geometric features of the bucket are manually annotated to train the semantic segmentation network model; the following steps are included: manually annotating the collected point cloud data and images, marking the geometric feature area of ​​the bucket, for example, marking the edge and corner points of the bucket in the point cloud, and marking the outline of the bucket in the image; converting the annotated point cloud data and image into a format suitable for training, such as the input point cloud features and corresponding labels; semantic segmentation network architecture, offline training of the annotated data, and optimization of network parameters to improve the model's segmentation accuracy for the bucket's geometric features; finally, using an independent validation set for testing, evaluating the performance of the model, and making necessary adjustments and optimizations. Assume the input image is , the semantic segmentation output is ,in is a binary image representing the bucket area. Extract feature points from the results of semantic segmentation To perform pose estimation, a convolutional neural network (CNN) is used for feature extraction: Based on the extracted features, a pose estimation network is constructed , output the six-degree-of-freedom position of the bucket (Location( ) and rotation ( )): Pose Estimation Network : The estimated pose is unified into a global coordinate system, and the following coordinate system transformation is assumed: The transformation matrix from the local coordinate system (bucket coordinate system) to the world coordinate system is , which contains rotation and translation information. Conversion formula: Integrating the above steps, we get the final bucket pose estimation formula:

9. A method for real-time measurement of the position and posture of a mining robot working device according to claim 2, characterized in that: The combined step S1 and step S2 perform pose estimation on the working device of the mining excavator robot, including: integrating the results of step S1 and step S2, fusing the joint pose information obtained by the body pose sensor and the bucket pose information provided by the environmental perception sensor through a factor graph optimization algorithm, and performing comprehensive pose estimation on the working device of the mining excavator robot. Nodes of the factor graph: The working device of the mining robot has three joints, and the pose information of the joints and bucket position information ; Factors (constraints) include entity sensor factors and environmental sensor factors. Ontological sensor factor: The encoder constraint factor is defined as in, It is The measurement values ​​obtained from the encoder for each joint, is the true angle of the joint calculated by the kinematic model; The inclinometer constraint factor is defined as: in Excavator working device Measurements obtained from the inclinometer for each joint; The Imu constraint factor is defined as: in, It is the current moment The location of the working device, It was the moment before location, It was the moment before The velocity is obtained by integrating the acceleration and time, is the acceleration at the previous moment, It is the time interval between the current moment and the previous moment. The camera constraint factor is defined as: in, It is a two-dimensional point measured by a camera installed on the working device. is the camera’s intrinsic parameter matrix, is the homogeneous transformation matrix of the camera's pose, are camera feature points. Environmental sensor factors: LiDAR Constraint Factors: in, Represents the transformation from the world coordinate system to the lidar coordinate system, Indicates the current position of the mining robot. Indicates the position of the model in the robot coordinate system (such as a geometric feature point of the bucket), It is the predicted position of the lidar feature point in the world coordinate system calculated by the current state estimation. Depth camera constraint factor: in, represents the pose of the depth camera, represents the depth value of the 3D point projected from the bucket position to the image plane, Indicates the depth value obtained by the depth camera. is the camera’s intrinsic parameter matrix, and are the rotation matrix and displacement vector of the camera respectively. The final optimization objective function is: 。

Images