A tunneling machine arm base vibration compensation and end precise positioning system and method

Abstract

The present application relates to the technical fields of engineering machinery automation control, and discloses a tunneling mechanical arm base vibration compensation and end precise positioning system and method, comprising multi-source sensing fusion, dynamics modeling, online learning feedforward and adaptive impedance control module.System receives base inertial navigation and end vision data, and outputs base six-degree-of-freedom motion state after filtering;Substitute it into the floating base dynamics model, decouple out the equivalent joint disturbance torque generated by the base vibration;Use neural network and Gaussian process regression to construct a hybrid network architecture, predict the feedforward compensation torque based on real-time vibration signals;Finally, estimate the surrounding rock stiffness to dynamically update the impedance parameters, and generate the joint driving torque by fusing the feedforward compensation torque.The present application can eliminate chassis vibration interference from the root of dynamics, change the control mode from passive feedback to active predictive compensation, and effectively improve the positioning accuracy and contact flexibility of the mechanical arm in complex working conditions.

Description

Technical Field

[0001] This invention relates to the field of automated control technology for engineering machinery, specifically to a vibration compensation and precise end-effector positioning system and method for a tunneling robot arm base. Background Technology

[0002] In coal mine or underground tunnel excavation operations, tunneling robots are often used to perform tasks such as rock drilling and anchor bolt support. Due to the complex underground geological environment and the heavy-duty cutting operations of the tunneling equipment itself, the moving base of the robot will generate multi-dimensional continuous spatial vibration.

[0003] Existing robotic arm control systems are mostly designed based on the assumption of a fixed base, without in-depth modeling of the dynamic coupling relationship between the floating base and the robotic arm joints. In actual working conditions, the six-degree-of-freedom spatial sway of the base transmits complex inertial forces and dynamic torque impacts to each robotic arm joint. Because current technology cannot accurately isolate and quantify these equivalent joint disturbance torques transmitted by the chassis, the robotic arm struggles to maintain end-effector pose stability under strong vibration environments, resulting in a significant decrease in positioning accuracy.

[0004] Meanwhile, traditional robotic arm control schemes mostly employ passive feedback adjustment mechanisms based on error elimination. This control method has an inherent time lag, and when faced with nonlinear and sudden base vibration impacts, it cannot predict the evolution trend of interference signals in advance and provide feedforward compensation, making it difficult to effectively suppress transient dynamic positioning deviations.

[0005] Furthermore, during the operation phase where the robotic arm's end effector contacts the roadway rock surface, existing technologies typically employ a fixed-parameter impedance control strategy. Because the stiffness of the surrounding rock at the actual working face varies significantly across different areas, fixed virtual stiffness and damping parameters cannot adaptively adjust to changes in the contact medium. This singular control mode results in poor compliance of the robotic arm's end effector when contacting rocks of varying hardness, easily leading to rigid collisions and uncontrolled contact forces. This not only increases the risk of damage to the equipment's mechanical structure but also reduces the overall quality of the engineering operation. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a system and method for vibration compensation and precise end-effector positioning of a tunneling manipulator base. It solves the problems of low positioning accuracy caused by the failure of existing control systems to consider the dynamic coupling interference caused by the vibration of the floating base, the lag in passive feedback control which cannot compensate for sudden transient impacts in advance, and the poor contact compliance of the manipulator and the tendency to cause rigid collisions due to the inability of fixed impedance control parameters to adapt to complex changes in the stiffness of the surrounding rock.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] The first aspect of this invention provides a vibration compensation and end-effector precision positioning system for a tunneling robot arm base, comprising: The multi-source sensor fusion module is used to receive the three-axis acceleration and three-axis angular velocity of the base collected by the base inertial measurement unit and the absolute pose data obtained by the end vision sensor, perform filtering calculations and obtain the six-degree-of-freedom motion state of the base and the absolute pose of the end of the robotic arm. The floating base dynamics modeling module is used to receive the six-degree-of-freedom motion state of the base and, in combination with the physical structural parameters of the robotic arm itself, separate the equivalent joint disturbance torque caused by the base vibration. The online learning feedforward compensation module is used to receive the triaxial acceleration and triaxial angular velocity of the base as real-time base vibration signals and the equivalent joint disturbance torque, establish a disturbance prediction model and calculate the comprehensive feedforward compensation torque vector. An adaptive impedance control module is used to combine the integrated feedforward compensation torque vector and the set impedance control parameters to generate joint driving torques for driving each joint of the robotic arm.

[0009] Preferably, the multi-source sensor fusion module is specifically used for: The raw acceleration data and raw angular velocity data output by the base inertial measurement unit are received, and the pure motion acceleration of the base itself is extracted as a three-axis acceleration vector through coordinate transformation and gravity compensation calculation. The extracted triaxial acceleration vectors and the original angular velocity data are subjected to low-pass filtering to output the smoothed triaxial acceleration and triaxial angular velocity of the base. Image feature extraction is performed on the image sequence acquired by the end vision sensor to calculate the absolute pose data of the robotic arm end relative to fixed feature points in the roadway at the current moment. Time series alignment is performed on the smoothed base triaxial acceleration, triaxial angular velocity, and absolute pose data to output an aligned heterogeneous sensing data set.

[0010] Preferably, the multi-source sensing fusion module is further used for: Perform extended Kalman filtering operations using the aligned heterogeneous sensor data set; Construct a system state vector that includes the base's three-dimensional position, three-dimensional velocity, three-dimensional acceleration, attitude quaternion, and three-axis angular velocity in the global reference frame; Using the base triaxial acceleration and triaxial angular velocity output from the preprocessing stage as system input, the prior state vector and prior covariance matrix at the current moment are recursively calculated; The absolute pose of the end effector is combined with the inverse matrix of the homogeneous transformation matrix of each link of the robotic arm to calculate the global pose observation of the base. The Kalman gain is calculated using the observation and the prior state vector is corrected for error. The smoothed six-degree-of-freedom displacement, velocity and acceleration data of the base are then extracted.

[0011] Preferably, the floating base dynamics modeling module is specifically used for: Construct a generalized coordinate vector for the composite system of the base and the robotic arm, wherein the generalized coordinate vector includes the six-degree-of-freedom pose vector of the base and the angle vector of the actual joint of the robotic arm; Based on the generalized coordinate vector, the total kinetic energy and total potential energy of the composite system are calculated, the Lagrangian function is constructed, and a global dynamic equation is established that includes the composite system's inertial matrix, Coriolis force and centrifugal force matrices, gravity term matrix, and driving torque mapping matrix.

[0012] Preferably, the floating base dynamics modeling module is further used for: The global dynamic equations are subjected to matrix partitioning and variable decoupling operations. The six-degree-of-freedom displacement, velocity and acceleration data of the base are used as known time-varying input variables, and the angular displacement, angular velocity and angular acceleration of the actual joints of the robotic arm are used as system state variables to be solved. The dynamic equations of the pure joint space are separated, and an analytical formula is generated to quantify the equivalent joint disturbance torque that generates dynamic torque impact on the joint shaft ends of the robotic arm due to the six-degree-of-freedom spatial vibration of the base.

[0013] Preferably, the online learning feedforward compensation module is specifically used for: A hybrid network architecture consisting of a one-dimensional convolutional neural network, a long short-term memory network, and a Gaussian process regression layer is constructed. The one-dimensional convolutional neural network is used to extract the local abrupt change features of the real-time base vibration signal within a short time interval; The local mutation features are received through the long short-term memory network, the temporal state information of the real-time base vibration signal under a long historical period is extracted, and a high-dimensional hidden state vector is output. The high-dimensional hidden state vector is mapped to the Gaussian process regression layer to infer the predicted mean and predicted variance of the equivalent joint disturbance torque.

[0014] Preferably, the online learning feedforward compensation module is further used for: An exponential decay function is constructed based on the predicted variance output by the Gaussian process regression layer. Dynamic weighting coefficients are calculated and applied to the predicted mean and the equivalent joint disturbance torque parsed in real time by the floating base dynamic modeling module. The weighted and fused outputs the comprehensive feedforward compensation torque vector. The mean of the equivalent joint disturbance torque of the prediction output is calculated in real time, and the numerical residual between the base vibration and the equivalent joint disturbance torque mapped to each joint is calculated. When the absolute value of the numerical residual exceeds the set safety error threshold within a preset number of control cycles, it is determined that the current prediction model has undergone local degradation, and an online update operation is triggered to perform Gaussian support set reconstruction and fine-tuning of the underlying network parameters.

[0015] Preferably, the adaptive impedance control module is specifically used for: The end contact force data collected by the six-dimensional force sensor at the end of the robotic arm and the end position deviation data calculated based on the joint encoder data of the robotic arm are obtained. Based on the ratio of the contact force increment to the position deviation increment, combined with a first-order low-pass filtering algorithm, the equivalent stiffness of the current contacting surrounding rock is estimated. The virtual stiffness matrix and virtual damping matrix are dynamically adjusted based on the equivalent stiffness. The updated virtual stiffness matrix and the virtual damping matrix replace the corresponding constant matrix in the preset basic impedance feedback control law in real time, and combined with the comprehensive feedforward compensation torque vector, output the dynamically adjusted final joint driving torque command vector.

[0016] Preferably, the adaptive impedance control module is further used for: Extract the angle error vector and angular velocity error vector of the robotic arm joint and construct a proportional differential sliding surface by linear combination; Extract the Gaussian process regression results output by the online learning feedforward compensation module, and use the prediction variance as an adjustment variable to calculate the dynamic robust gain in the sliding mode gain function. The sliding mode robust compensation torque vector is calculated using the hyperbolic tangent function. This vector is then superimposed onto the existing feedforward and impedance fusion control framework to update the final control execution command. The final joint drive torque command vector is linearly mapped to the corresponding desired current signal, which is then sent as the given input of the current loop to the underlying servo driver of each joint.

[0017] A second aspect of the present invention provides a method for vibration compensation and precise end-effector positioning of a tunneling robot arm base, comprising the following steps: The base inertial measurement unit collects the triaxial acceleration and triaxial angular velocity of the base in real time, and the absolute pose data is obtained through the end vision sensor; The multi-source sensor fusion module receives the three-axis acceleration, the three-axis angular velocity, and the absolute pose data of the base, performs filtering calculations, and outputs the six-degree-of-freedom motion state of the base. The floating base dynamics modeling module receives the six-degree-of-freedom motion state of the base and separates the equivalent joint disturbance torque by combining it with the physical structural parameters of the robotic arm itself. The online learning feedforward compensation module receives the triaxial acceleration and triaxial angular velocity of the base as real-time base vibration signals, and receives the equivalent joint disturbance torque, establishes a disturbance prediction model, and calculates the comprehensive feedforward compensation torque vector. The adaptive impedance control module combines the integrated feedforward compensation torque vector and the set impedance control parameters to generate joint driving torques for driving each joint of the robotic arm, and outputs them to the underlying servo drivers of each joint of the robotic arm to drive the robotic arm to complete the positioning action.

[0018] This invention provides a vibration compensation system and method for the base of a tunneling robot and precise end-effector positioning. It offers the following advantages: 1. This invention can eliminate the impact of base vibration on the positioning accuracy of the robotic arm in complex tunnel environments. By combining a multi-source sensor fusion module with an extended Kalman filter, the system can accurately extract the six-degree-of-freedom motion state of the base itself. Based on this, a global dynamic equation is constructed using a floating base dynamics modeling module, and variables are decoupled. The spatial vibration of the base is quantified and transformed into equivalent joint disturbance torques acting on each joint, accurately extracting the specific disturbance values ​​of chassis sway on the end effector, thus improving the positioning accuracy of the robotic arm under strong vibration conditions from the root of dynamics.

[0019] 2. This invention overcomes the control lag problem of traditional feedback control systems when facing sudden impacts. The system adopts a hybrid network architecture of one-dimensional convolutional neural network, long short-term memory network, and Gaussian process regression cascade to learn the collected base vibration signal online. This module can simultaneously process the local abrupt change characteristics and long-term state of the vibration signal, predict the mean and variance of the disturbance torque in advance, and then generate a feedforward compensation torque. This transforms the control system from passive disturbance rejection to active prediction and compensation, offsetting the dynamic error of the robotic arm when facing nonlinear impact loads.

[0020] 3. This invention improves operational safety and contact compliance when the robotic arm's end effector contacts the surrounding rock in the tunnel. The adaptive impedance control module estimates the equivalent stiffness of the current working face's surrounding rock in real time by reading the incremental ratio of contact force to position deviation, and dynamically updates the virtual stiffness and virtual damping parameters in the impedance control model accordingly. This is then fused with the feedforward compensation torque and the sliding mode control robust term to generate the final joint driving torque, enabling the end effector to automatically adjust its compliance according to the rock surface's hardness, thus avoiding mechanical collision damage that is prone to occur under rigid position control. Attached Figure Description

[0021] Figure 1This is a system architecture diagram of the present invention; Figure 2 This is a flowchart of the method of the present invention; Figure 3 This is a flowchart of the multi-source sensor fusion and heterogeneous data preprocessing process of the present invention; Figure 4 This is a comparison chart of the end-point error indices for resisting broadband vibration interference under different control strategies of the present invention; Figure 5 This is a comparison diagram of the torque response curves during the contact stage of the variable stiffness surrounding rock according to the present invention. Detailed Implementation

[0022] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0023] See attached document Figure 1 This invention provides a vibration compensation and end-effector precision positioning system for a tunneling robot arm base, comprising: The system includes a multi-source sensor fusion module, a floating base dynamics modeling module, an online learning feedforward compensation module, and an adaptive impedance control module.

[0024] The system is equipped with a base inertial measurement unit (IMU) and an end-effector vision sensor on the hardware side. The IMU is mounted on the robot arm base and is used to collect the triaxial acceleration and triaxial angular velocity of the base. The end-effector vision sensor is mounted on the end of the robot arm and is used to acquire the pose data of the end of the robot arm relative to fixed feature points in the roadway.

[0025] The data output terminals of the base inertial measurement unit and the end-effector vision sensor are connected to the input terminal of the multi-source sensor fusion module. The multi-source sensor fusion module performs filtering operations on the received sensor data to obtain the six-degree-of-freedom motion state of the base and the absolute pose of the robotic arm's end effector.

[0026] The output of the multi-source sensor fusion module is connected to the floating base dynamics modeling module. Based on the input six-degree-of-freedom motion state of the base and the physical structural parameters of the robotic arm itself, the floating base dynamics modeling module separates the equivalent joint disturbance torque caused by the base vibration.

[0027] The output of the floating base dynamics modeling module and the base inertial measurement unit are respectively connected to the online learning feedforward compensation module. The online learning feedforward compensation module receives real-time base vibration signals and equivalent joint disturbance torques, establishes a disturbance prediction model, and calculates the feedforward compensation torque.

[0028] The online learning feedforward compensation module is connected to the adaptive impedance control module. The adaptive impedance control module combines the feedforward compensation torque and the set impedance control parameters to generate and output the joint driving torque for driving each joint of the robotic arm.

[0029] See attached document Figure 2 This invention provides a method for vibration compensation and precise end-effector positioning of a tunneling robot arm base. Based on the above system composition, the workflow of this method is as follows: The base's three-axis acceleration and three-axis angular velocity are acquired in real time by the base inertial measurement unit, and the pose data of the robotic arm's end effector is obtained by the end-effector vision sensor. The multi-source sensor fusion module receives the acquired data, performs extended Kalman filtering, and outputs the six-degree-of-freedom motion state of the base after data fusion processing.

[0030] The floating base dynamics modeling module constructs the dynamics model of the system's floating base. The six-degree-of-freedom motion state of the base, output by the multi-source sensor fusion module, is substituted into this model to calculate the equivalent joint disturbance torque expression of the base vibration acting on each joint of the robotic arm.

[0031] The online learning feedforward compensation module acquires the base vibration signal output in real time from the base inertial measurement unit and inputs this signal into the established online learning model. The online learning model performs predictive calculations based on the input data, calculates the current equivalent joint disturbance torque value, and generates a feedforward compensation torque accordingly.

[0032] See attached document Figure 2 The adaptive impedance control module estimates the surrounding rock stiffness of the robotic arm's current working area online and dynamically adjusts the stiffness and damping parameters of the impedance controller based on the estimated rock stiffness. Subsequently, the adaptive impedance control module superimposes the feedforward compensation torque output by the online learning feedforward compensation module into the control law formula of the impedance controller.

[0033] The adaptive impedance control module dynamically adjusts the gain coefficient within the controller based on the robust term in sliding mode control and the prediction variance output by the online learning model. After final numerical solution, the adaptive impedance control module outputs joint driving torque to the servo motor of the robotic arm, completing the positioning action of the robotic arm's end effector.

[0034] See attached document Figure 3 Before the multi-source sensor fusion module performs extended Kalman filtering, it receives raw data from the base inertial measurement unit and the end-effector vision sensor, and performs preprocessing on the raw data. This heterogeneous sensor data preprocessing stage includes the following specific steps: The multi-source sensor fusion module reads the raw acceleration and angular velocity data output by the base inertial measurement unit (IMU). Since the IMU is fixed to the robotic arm base, its coordinate system has an installation offset from the system's global reference system, and the raw acceleration data includes a component of Earth's gravitational acceleration. The multi-source sensor fusion module extracts the base's own pure motion acceleration through coordinate transformation and gravity compensation calculations. The corresponding calculation formula is expressed as: ; in, The three-axis acceleration vector in the base reference frame extracted by the multi-source sensor fusion module; The rotation matrix is ​​the transformation from the sensor body coordinate system to the base reference system. This rotation matrix is ​​obtained through the initial equipment calibration of the system, for example, it can be calculated using a conventional hand-eye calibration algorithm. This is the original acceleration vector output by the base inertial measurement unit; This is the gravitational acceleration vector transformed into the base reference frame.

[0035] The multi-source sensor fusion module performs low-pass filtering on the extracted triaxial acceleration vectors and raw angular velocity data to eliminate high-frequency environmental noise interference caused by the high-speed rotation of the tunneling machine's hydraulic pump station and cutting head. After low-pass filtering, the multi-source sensor fusion module outputs smoothed triaxial acceleration and triaxial angular velocities of the base. The structural design of the low-pass filter and the configuration of its cutoff frequency parameters can be conventionally set by those skilled in the art based on the vibration spectrum data collected during the actual operation of the tunneling equipment. The specific digital signal filtering implementation process is well-known in the field and will not be elaborated here.

[0036] A multi-source sensor fusion module acquires image sequences captured by an end-effector vision sensor. In one specific embodiment, the end-effector vision sensor is a binocular industrial camera or an RGB-D depth camera fixedly mounted on the flange at the end of the robotic arm. The multi-source sensor fusion module performs image feature extraction on the input image sequence to identify fixed feature points in the tunnel within the field of view. Further, the fixed feature points are laser reflective targets pre-anchored to the tunnel roof, or reference crosshairs projected onto the working rock surface by a tunnel laser pointer. Based on the pixel coordinates of the extracted feature points on the image plane, and combined with the intrinsic parameter matrix of the end-effector vision sensor, the multi-source sensor fusion module calculates the absolute pose data of the robotic arm end relative to the fixed feature points in the tunnel at the current moment. This absolute pose data includes a three-dimensional translation vector representing the position and a quaternion vector representing the posture. For the feature point extraction and spatial pose calculation process based on the vision camera, those skilled in the art can use perspective point localization algorithms or existing visual odometry frameworks; the underlying calculation logic is well-known in the field and will not be elaborated here.

[0037] The multi-source sensor fusion module performs time-series alignment on the pre-processed triaxial acceleration, triaxial angular velocity of the base, and the absolute pose of the robotic arm's end effector. The hardware sampling frequency of the base's inertial measurement unit (IMU) is typically higher than the image acquisition frequency of the end-effector's vision sensor. The multi-source sensor fusion module extracts the IMU sampling data frame closest to the exposure time of the vision image on the timeline and calculates the base acceleration and angular velocity data that perfectly correspond to the vision sensor's timestamp using a linear interpolation algorithm. Through this time synchronization mechanism, the multi-source sensor fusion module outputs an aligned heterogeneous sensor data set, which is then input into the subsequent tightly coupled filtering stage.

[0038] After preprocessing the heterogeneous sensor data, the multi-source sensor fusion module performs an extended Kalman filter operation using the aligned heterogeneous sensor data set. This tightly coupled filter design phase includes the following specific processing steps: The multi-source sensor fusion module constructs the system state vector of the extended Kalman filter. To fully characterize the base's motion state and its first and second derivatives, the system state vector defined within the multi-source sensor fusion module includes the base's three-dimensional position, three-dimensional velocity, three-dimensional acceleration, attitude quaternion, and three-axis angular velocity in the global reference frame. Specifically, the three-dimensional velocity corresponds to the first derivative at position, the three-dimensional acceleration corresponds to the second derivative at position, and the three-axis angular velocity corresponds to the rate of change of the attitude parameters.

[0039] The multi-source sensor fusion module uses the integration of the base inertial measurement unit (IMU) for state prediction. Specifically, the module uses the base's three-axis acceleration and three-axis angular velocity output from the preprocessing stage as system input, and recursively calculates the prior state vector and prior covariance matrix at the current time step using the system's kinematic differential equations over time. The calculation formula for this state prediction step is expressed as: ; ; in, For the current moment The prior state vector; For the previous moment The posterior state vector; This is the input vector from the previous moment, namely the triaxial acceleration and triaxial angular velocity of the base; It is a nonlinear state transition function containing the integration logic of inertial data; This is the process noise vector of the system; This is the prior covariance matrix predicted at the current time. The Jacobian matrix is ​​obtained by taking the partial derivative of the nonlinear state transition function with respect to the state vector. Let be the posterior covariance matrix of the previous time step; The transpose of the Jacobian matrix obtained by taking the partial derivative of the nonlinear state transition function with respect to the state vector; Let be the process noise covariance matrix.

[0040] The multi-source sensor fusion module uses the end-effector's visual pose as the observation update. Since the visual sensor directly acquires the pose parameters of the robotic arm's end effector, the multi-source sensor fusion module reads the angle feedback values ​​of each joint in real time through the robotic arm controller's communication interface. Substituting these values ​​into the robotic arm's forward kinematics equations and combining them with the inverse of the homogeneous transformation matrices of each link, the absolute pose of the end effector is reverse-mapped and calculated as the global pose observation value of the base. Subsequently, the multi-source sensor fusion module uses this observation value to calculate the Kalman gain and corrects the error in the predicted prior state vector. The corresponding observation update formula is expressed as: ; ; in, The calculated Kalman gain matrix; The Jacobian matrix corresponding to the observation equation; Let be the transpose of the Jacobian matrix corresponding to the observation equation; Let be the observation noise covariance matrix of the visual sensor; This is the posterior state vector output after correction based on the observed data; To acquire and map the actual observation vectors for the vision sensor; This is a nonlinear observation function that transforms the state space into the observation space.

[0041] The multi-source sensor fusion module completes a single filtering iteration. The multi-source sensor fusion module then processes the posterior state vector obtained from the solution. The relevant components are extracted, and the smoothed base motion state and its first and second derivatives are output. This filtered output data eliminates the cumulative drift error caused by long-term integration by the inertial measurement unit. The multi-source sensor fusion module transmits the smoothed six-degree-of-freedom displacement, velocity, and acceleration data of the base to the downstream floating base dynamics modeling module. The specific process of solving the partial derivatives of the Jacobian matrix in the extended Kalman filter and the matrix inversion operation can be implemented by those skilled in the art using conventional numerical analysis theory and matrix algebra methods. The underlying matrix operation logic is well-known in the field and will not be elaborated here.

[0042] The multi-source sensor fusion module outputs the smoothed six-DOF motion state data of the base to the floating base dynamics modeling module. The floating base dynamics modeling module receives this state data and performs Lagrangian dynamics modeling of the composite system. This dynamics modeling stage includes the following specific processing steps: The floating base dynamics modeling module constructs the generalized coordinate vector of the composite system of the base and the robotic arm. Unlike the pure joint space of a fixed base, the floating base dynamics modeling module equates the six degrees of freedom motion of the base in three-dimensional space to the motion of six virtual joints, specifically three orthogonal translational virtual joints and three orthogonal rotational virtual joints connected in series. These six virtual joint variables are then concatenated with the actual joint variables of the robotic arm. The generalized coordinate vector of the composite system is represented as follows: . This is a generalized coordinate vector that includes the base posture and the robot arm joint angles. The pose vector of the base is a six-degree-of-freedom vector, which includes three translation variables and three rotation variables; Let be the angle vectors of each actual joint of the robotic arm. The first derivative of the generalized coordinate vector. With the second derivative These represent the generalized velocity vector and the generalized acceleration vector of the system, respectively.

[0043] The floating base dynamics modeling module calculates the total kinetic energy and total potential energy of the composite system based on the aforementioned generalized coordinate vectors. According to the mass, center of mass position, and inertia tensor parameters of each link of the robotic arm, combined with the kinematic Jacobian matrix of the robotic arm system, the module calculates the translational and rotational kinetic energy of the base body and the kinetic energy of each link of the robotic arm, and sums these kinetic energies to obtain the total kinetic energy of the system. Simultaneously, the module calculates the total potential energy of the system in the gravitational field based on the height of the center of mass of each link in the global reference frame. The module constructs a Lagrangian function, the value of which is equal to the difference between the total kinetic energy and the total potential energy of the system. For the specific calculation rules of the kinetic and potential energy of multi-rigid-body systems, those skilled in the art can write algorithms based on the link recursive formulas in robotics theory to solve them; this is well-known technology in the field and will not be elaborated here.

[0044] The floating base dynamics modeling module uses the Lagrange method to establish the dynamic equations in generalized coordinates. The module substitutes the constructed Lagrange function into the second kind of Lagrange equation to derive partial derivatives, establishing a dynamic model of the composite system of the base and robotic arm considering base motion disturbances. The corresponding global dynamics calculation formula is expressed as: ; The floating base dynamics modeling module confirms the matrix parameters in the above dynamic equations. In this formula, The inertial matrix of the composite system is used to describe the inertial coupling relationship between the base itself, the robotic arm itself, and the base and the robotic arm. The matrix represents the Coriolis force and centrifugal force, reflecting the centripetal and Coriolis force effects generated by the linkage motion within the system. The gravity term matrix represents the projected components of the system's gravity in each generalized coordinate direction; This is the driving torque mapping matrix; This is the actual driving torque input to the joint.

[0045] On the right side of the equation, since the tunneling machine base experiences passive vibration rather than active driving at the tunneling face, the driving torque mapping matrix... It is represented as a diagonal block matrix. Within this matrix, the blocks corresponding to the base's motion degrees of freedom are zero matrices, while the blocks corresponding to the actual joint degrees of freedom of the robotic arm are identity matrices. The actual driving torque input to the joints... The vector dimension matches the actual number of joints in the robotic arm. The floating base dynamics modeling module completes the dynamic mathematical modeling of the base and robotic arm under a unified physical framework by determining the above matrix structure, providing a foundation for subsequent matrix decoupling.

[0046] After establishing the dynamic model of the composite system of the base and robotic arm, the floating base dynamic modeling module performs matrix partitioning and variable decoupling operations on the global dynamic equations. This joint space equation partitioning and decoupling stage includes the following specific processing steps: The floating base dynamics modeling module classifies the generalized coordinate vectors and their derivatives of the composite system based on the physical properties of the system's degrees of freedom. The overall system degrees of freedom consist of the six passive degrees of freedom of the base and the multiple active driving degrees of freedom of the robotic arm. The floating base dynamics modeling module uses the six-degree-of-freedom displacement, velocity, and acceleration data of the base output by the multi-source sensor fusion module as known time-varying input variables, and the angular displacement, angular velocity, and angular acceleration of the actual joints of the robotic arm as the system state variables to be solved, thereby distinguishing the vibration excitation source and the controlled body in the mathematical model.

[0047] The floating base dynamics modeling module divides the matrices of the global dynamic equations into blocks. Based on the specific arrangement of the generalized coordinate vectors, the module splits the composite system's inertia matrix, Coriolis force matrix, and centrifugal force matrix into block matrices. The resulting sub-matrices correspond to pure base space blocks, pure joint space blocks, and cross-coupling blocks between the base and joints. The pure base space block has a dimension of 6×6, and the pure joint space block has a dimension of... , Given the actual number of joints in the robotic arm, the dimension of the cross-coupling block is 6× and ×6. Simultaneously, the floating base dynamics modeling module splits the gravity term matrix and driving torque vector into two column vector blocks according to the base dimension and joint dimension. Through matrix block operation, the original single global high-dimensional differential equation system is transformed into two interrelated low-dimensional differential equation systems.

[0048] The floating base dynamics modeling module extracts and constructs the dynamic equations for the pure joint space. This module discards the upper-level equation blocks corresponding to the passive degrees of freedom of the base and extracts the lower-level equation blocks corresponding to the actual driving degrees of freedom of the robotic arm. In the extracted lower-level equations, terms containing the joint state variables of the robotic arm are retained on the left-hand side, while terms containing the known motion state variables of the base are moved to the right-hand side, completing the algebraic separation of vibration disturbance terms and joint body terms. The resulting dynamic equations for the pure joint space are expressed as follows: ; in, and These are the angular acceleration and angular velocity vectors of the robotic arm joints, respectively. , , These are the joint space inertia matrix, Coriolis force matrix, and gravity vector after decoupling, respectively. This refers to the actual driving torque input to the joint. This represents the equivalent joint disturbance torque mapped from the base vibration to each joint.

[0049] The floating base dynamics modeling module generates an analytical formula for the equivalent joint disturbance torque based on the rearrangement of the right side of the equation. This analytical formula is used to quantify the dynamic torque impact generated by the six-degree-of-freedom spatial vibration of the base on the joint ends of each robotic arm. The analytical expression for the equivalent joint disturbance torque is extracted as follows: ; In this formula, and These are the six-DOF acceleration and velocity vectors of the base, output by the sensor fusion module, respectively. and The coupling inertia matrix and coupling Coriolis force matrix between the base and the joint are defined. The floating base dynamics modeling module outputs the calculated equivalent joint disturbance torque data sequence to the online learning feedforward compensation module to establish the target variable for feedforward compensation. The linear algebra calculation rules and submatrix extraction algorithms in the matrix partitioning process can be implemented using conventional numerical calculation software or matrix operation libraries by those skilled in the art; these are well-known techniques in the field and will not be elaborated upon here.

[0050] After completing the block division and decoupling of the joint space equations, the floating base dynamics modeling module further performs real-time analytical and quantitative calculations of the separated equivalent joint disturbance torques. This equivalent joint disturbance torque analysis stage includes the following specific processing steps: The floating base dynamics modeling module, based on the extracted analytical expression of the equivalent joint disturbance torque, updates and calculates the coupled inertia matrix and coupled Coriolis force matrix in real time. The coupled inertia matrix between the base and the joint... and the coupled Coriolis force matrix Instead of a fixed constant matrix, it is a time-varying matrix dependent on the instantaneous poses of the current robotic arm and the base. The floating base dynamics modeling module reads the absolute encoder values ​​of the servo motors of each joint in real time through the servo communication bus at the bottom of the robotic arm to obtain the current actual joint angles of the robotic arm. Combined with the current base pose parameters synchronously output by the multi-source sensor fusion module, the floating base dynamics modeling module calls the internally pre-stored dynamic parameters such as the mass and inertia tensor of each link of the robotic arm, as well as the geometric parameters of the DH link, to re-evaluate and refresh the data. and The numerical values ​​of each matrix element within the model. For solving the elements and performing partial derivative operations of the time-varying coupling matrix in a multi-rigid-body dynamics model, those skilled in the art can implement this by writing a low-level solver based on the Newton-Euler method or the Lagrange recursive method formula. The underlying algorithm logic is well-known in this field and will not be elaborated here.

[0051] The floating base dynamics modeling module performs matrix algebra operations and generates a discrete torque data sequence. It substitutes the real-time updated coupled inertia matrix, coupled Coriolis force matrix, and the base's six-degree-of-freedom acceleration and velocity vectors into the aforementioned analytical expression, performing matrix multiplication and addition operations to calculate the equivalent joint disturbance torque values ​​experienced by each joint axis within the current control cycle. As the tunneling operation continues, the floating base dynamics modeling module outputs a time-continuous equivalent joint disturbance torque data sequence according to the system's set control frequency. This data sequence is buffered and stored, and then transmitted in real-time to the downstream online learning feedforward compensation module to establish target variables for feedforward compensation.

[0052] The online learning feedforward compensation module receives real-time base vibration signals and the equivalent joint disturbance torque calculated by the floating base dynamics modeling module, and establishes a disturbance prediction model. The design phase of this online learning network architecture includes the following specific processing steps: The online learning feedforward compensation module constructs a hybrid network architecture with input and output layers. In one specific implementation, this hybrid network architecture consists of a cascaded one-dimensional convolutional neural network, a long short-term memory network, and a Gaussian process regression layer. The input data for the input layer is a time-series matrix composed of the three-axis accelerations and three-axis angular velocities of the base, continuously collected by the base inertial measurement unit within a set time window. Specifically, the set time window is a sliding time window that includes the current moment and a preset number of past sampling periods. The output data for the output layer is the predicted mean and predicted variance of the equivalent joint disturbance torque for the corresponding control period. The predicted mean is used for feedforward compensation, and the predicted variance is used to quantify the current prediction uncertainty of the model, providing adjustment parameters for subsequent adaptive control.

[0053] The online learning feedforward compensation module utilizes a one-dimensional convolutional neural network to construct a local feature extraction layer. The input time-series matrix undergoes sliding computation through a one-dimensional convolutional kernel to extract local abrupt changes in the vibration signal within a short time interval, generating a preliminary feature mapping vector. The calculation formula for the one-dimensional convolutional layer is expressed as follows: ; in, This represents the local feature vector output by a one-dimensional convolutional neural network. It is a linear rectification activation function; It is a one-dimensional convolution kernel weight matrix; This represents a one-dimensional convolution operator; The input layer receives a timing matrix containing the vibration signal within the current time and the past set time window; This is the bias vector of the convolutional layer.

[0054] The online learning feedforward compensation module utilizes a Long Short-Term Memory (LSTM) network to construct a temporal feature processing layer. Local feature vectors are input to each time step unit of the LTM network. Through internal forgetting, input, and output gate mechanisms, the temporal state information of the vibration signal over a long historical period is extracted, outputting a high-dimensional hidden state vector containing global temporal information. The core hidden state update formula of the LTM network is expressed as: ; in, This is the high-dimensional hidden state vector output by the Long Short-Term Memory network at the current time step; Represents the nonlinear operation function within a long short-term memory network unit; For the current moment The local feature vector output by a one-dimensional convolutional neural network; Let be the hidden state vector of the Long Short-Term Memory network at the previous time step; This is the cell state vector at the previous moment on the Long Short-Term Memory network.

[0055] The online learning feedforward compensation module utilizes a Gaussian process regression layer to output prediction results. This module maps a high-dimensional hidden state vector as input to the Gaussian process model. The Gaussian process regression layer calculates the similarity between the current input and historical training samples based on a kernel function, inferring the predicted mean and variance of the equivalent joint disturbance torque. This kernel function can be, for example, a radial basis function or a Matern kernel function. The prediction calculation formula is expressed as: ; ; in, To predict the mean of the equivalent joint disturbance torque of the output; To predict the variance of the output; The covariance vector between the current high-dimensional hidden state vector and the historical sample set; Let be the covariance matrix of the historical sample set; The pre-defined Gaussian observation noise variance; It is the identity matrix; This is the true equivalent joint disturbance torque data vector corresponding to the historical training samples; This is the high-dimensional hidden state vector currently input to the Gaussian process layer; This is the autocovariance scalar of the high-dimensional hidden state vector currently input to the Gaussian process layer. The specific structure of the neural network and the selection of the Gaussian process kernel function can be implemented using conventional deep learning frameworks by those skilled in the art. The underlying network forward propagation logic is well-known in the field and will not be elaborated upon here.

[0056] After constructing the hybrid network architecture, the online learning feedforward compensation module performs real-time feature input and forward prediction operations. This feature input and forward prediction stage includes the following specific processing steps: The online learning feedforward compensation module constructs a real-time feature data matrix for model input. Following a set system control cycle, the module reads the base's three-axis acceleration and three-axis angular velocity output from the multi-source sensor fusion module in real time, and sequentially concatenates the data from the current moment with the data from a preset number of past sampling cycles to form a time-series matrix within a sliding time window. To eliminate the influence of the different physical dimensions of acceleration and angular velocity on the accuracy of neural network feature extraction, the online learning feedforward compensation module performs standardization processing on this time-series matrix. The corresponding standardization calculation formula is expressed as follows: ; in, For the standardized process Data values ​​for each feature dimension; The first in the time series matrix The original input data for each feature dimension, where =1,2,...,6, which correspond to the six physical channels of the base's three-axis acceleration and three-axis angular velocity, respectively; For the first historical sample dataset The statistical mean of each feature dimension; For the first historical sample dataset The statistical standard deviations of each feature dimension are calculated. After standardization, the online learning feedforward compensation module outputs a dimensionless standard time series matrix.

[0057] The online learning feedforward compensation module performs forward propagation calculations using a one-dimensional convolutional neural network. It inputs a standard time-series matrix into the local feature extraction layer and performs a sliding inner product operation along the time dimension using an internally configured one-dimensional convolutional kernel. This process filters and reduces the dimensionality of high-frequency abrupt changes in the base vibration signal, and performs a nonlinear mapping to zero out negative values ​​using a linear rectified activation function to extract local feature vectors containing local vibration mode information.

[0058] The online learning feedforward compensation module performs the forward propagation calculation of the Long Short-Term Memory (LSTM) network. It uses the aforementioned local feature vectors as input variables for the current time step, and combines them with the hidden state vectors and cell state vectors retained within the LTM network model from the previous time step to sequentially calculate the state values ​​of the input gate, forget gate, and output gate at the current time step. Through nonlinear update operations of the cell states, the online learning feedforward compensation module extracts the time delay decay characteristics of the base vibration signal under long periods and outputs a high-dimensional hidden state vector representing global temporal information.

[0059] The online learning feedforward compensation module utilizes a Gaussian process regression layer to perform joint inference. The module inputs the aforementioned high-dimensional hidden state vector into the Gaussian process model, calculates the covariance vector between this high-dimensional hidden state vector and the historical support set samples stored within the model, and the autocovariance scalar of the high-dimensional hidden state vector based on the kernel function. Based on the Gaussian probability distribution theorem, the module calculates the posterior probability distribution of the equivalent joint disturbance torque at the current moment, extracts the expected value of this distribution as the predicted mean of the equivalent joint disturbance torque, and extracts the variance of this distribution as the predicted variance. This predicted mean serves as the system's feedforward compensation instruction. The forward multiplication and addition rules of the neural network tensor and the underlying calculation logic of the activation function can be implemented using conventional matrix operation libraries or deep learning acceleration frameworks by those skilled in the art; these are well-known technologies in the field and will not be elaborated upon here.

[0060] After obtaining the forward prediction results, the online learning feedforward compensation module combines the analytical results of the physical model to perform dynamic weighted fusion and online model update operations. This dynamic weighted fusion and online weight update stage includes the following specific processing steps: The online learning feedforward compensation module assesses prediction uncertainty and calculates dynamic weighting coefficients. Because changes in the hardness of the coal and rock at the working face can cause abrupt changes in the vibration excitation source on the robotic arm base, purely data-driven prediction models risk a decrease in confidence when facing unknown working conditions. The online learning feedforward compensation module constructs an exponentially decaying function based on the prediction variance output from the Gaussian process regression layer, mapping this variance to dimensionless weight values ​​in the interval [0,1]. As the prediction variance increases, the weighting coefficients decay exponentially, thereby mitigating the potential instability risk posed by high-uncertainty predictions. The formula for calculating the dynamic weighting coefficients is expressed as: ; in, For the current control cycle The calculated dynamic weighting coefficients; This is a preset variance penalty scaling factor used to adjust the sensitivity of the weighting coefficients to the decay of prediction uncertainty.

[0061] The online learning feedforward compensation module performs dynamic weighted fusion calculations on dual-source data. The predicted mean output by the hybrid network architecture overcomes the physical lag of the control system; while the equivalent joint disturbance torque, analyzed in real time by the floating base dynamics modeling module, is based on a deterministic physical model, ensuring system stability under extreme disturbances. The online learning feedforward compensation module applies dynamic weight coefficients to both sources respectively, and the calculated output is ultimately sent to the joint actuator as control commands. The weighted fusion calculation formula is expressed as: ; in, This is the comprehensive feedforward compensation torque vector output after dynamic weighted fusion. This comprehensive feedforward compensation torque vector is directly fed forward and superimposed into the torque command loop of the underlying servo controller of each joint of the robotic arm.

[0062] The online learning feedforward compensation module establishes an online monitoring and update triggering mechanism for model degradation. As tunneling operations progress, the vibration data distribution may deviate from the historical training data distribution. Within each control cycle, the online learning feedforward compensation module calculates in real time the numerical residual between the mean of the equivalent joint disturbance torque of the predicted output and the equivalent joint disturbance torque mapped from the base vibration to each joint. The module monitors the changing trend of this residual. When the absolute value of this residual exceeds the set safety error threshold within a preset number of control cycles (e.g., 5 to 10), the system determines that the current prediction model has undergone local degradation and triggers an online update operation.

[0063] The online learning feedforward compensation module performs Gaussian support set reconstruction and fine-tuning of underlying network parameters. For the Gaussian process regression layer, the online learning feedforward compensation module maps the currently extracted high-dimensional hidden state vector and the corresponding base vibration to the equivalent joint disturbance torque of each joint as a new joint data pair, adding it to the historical support set of the Gaussian process regression layer. To prevent unlimited expansion of the matrix dimension, the online learning feedforward compensation module implements a timestamp-based queue replacement strategy, removing the oldest data pair in the historical support set and recalculating the covariance matrix of the historical sample set based on the new support set. For the front-end feature extraction network, the online learning feedforward compensation module constructs a loss function using the sum of squares of the residuals and uses the gradient descent algorithm to perform backpropagation fine-tuning of the one-dimensional convolutional kernel weight matrix and the internal neuron parameters of the long short-term memory network. For the incremental inversion update algorithm of the Gaussian process covariance matrix and the backpropagation fine-tuning rule of the neural network weights, those skilled in the art can write algorithm programs based on conventional online learning theory. The underlying derivation process is a well-known technology in this field and will not be elaborated here.

[0064] The adaptive impedance control module receives the comprehensive feedforward compensation torque vector output by the online learning feedforward compensation module, and, combined with the actual motion state of the robotic arm, performs a composite control law model construction operation. This composite control law model construction stage includes the following specific processing steps: The adaptive impedance control module extracts the motion error state of the robotic arm joints. It acquires the desired motion trajectory data from the host planner via the system communication bus. This desired motion trajectory data is specifically a time series discretized according to the system control cycle, containing the desired joint angle, desired joint angular velocity, and desired joint angular acceleration for each interpolation cycle. The adaptive impedance control module uses encoder data fed back from the joint servo motors to calculate the deviation between the desired state and the actual state. The calculation formula is expressed as: ; ; in, This is the joint angle error vector; This is the joint angular velocity error vector; and These are the desired joint angle vector and the desired joint angular velocity vector, respectively. Let be the angular displacement vector of the robotic arm joint. This is the angular velocity vector of the robotic arm joint.

[0065] The adaptive impedance control module constructs a basic impedance feedback control law. To ensure the robotic arm's compliance with residual vibrations, the module establishes a virtual impedance model based on second-order differential equations. Combining this with the decoupled joint space dynamics model, the module derives the impedance feedback control torque using a calculated torque method; the corresponding formula is as follows: ; in, This is the impedance feedback control torque vector; The desired joint angle acceleration vector; , , These are preset virtual inertia matrix, virtual damping matrix, and virtual stiffness matrix, respectively. In one specific implementation, the aforementioned virtual inertia matrix, virtual damping matrix, and virtual stiffness matrix are all positive definite diagonal matrices, and their diagonal elements correspond to the impedance parameters of each independent joint of the robotic arm.

[0066] The adaptive impedance control module performs a superposition operation on the feedforward and feedback signals. It uses the predicted comprehensive feedforward compensation torque vector as the feedforward compensation term and the aforementioned impedance feedback control torque vector as the feedback adjustment term to calculate the final composite control command issued to the actuator. The mathematical expression of the composite control law is: ; in, This is the final joint driving torque command vector output by the composite control law. The adaptive impedance control module uses a pre-calibrated motor torque constant to linearly map this final joint driving torque command vector into the corresponding desired current signal, and sends it as the given input of the current loop to the underlying servo driver of each joint. The closed-loop regulation and space vector pulse width modulation process of the internal current loop of the servo driver can be implemented using conventional motor control theory by those skilled in the art; these are well-known techniques in the field and will not be elaborated upon here.

[0067] After constructing the composite control law model, the adaptive impedance control module further performs online estimation of the surrounding rock stiffness and impedance adaptation. This online estimation and impedance adaptation stage of the surrounding rock stiffness includes the following specific processing steps: The adaptive impedance control module acquires contact force and position deviation data at the end effector of the robotic arm. It reads the contact force data in real time using a six-dimensional force sensor mounted at the end effector and extracts the contact force component along the working axis of the tool coordinate system (e.g., the Z-axis). Using joint encoder data and a preset forward kinematics model, the module calculates the actual position of the end effector, performs forward kinematics calculations using the desired joint angle vector to obtain the corresponding desired position, and calculates the position deviation component between the two.

[0068] The adaptive impedance control module performs online estimation of the surrounding rock stiffness. Because the physical properties of the coal and rock at the working face change continuously with the tunneling depth, a constant impedance parameter can easily cause system oscillation or contact force overload in a variable stiffness environment. The adaptive impedance control module estimates the equivalent stiffness of the currently contacting surrounding rock based on the ratio of the contact force increment to the position deviation increment, combined with a first-order low-pass filtering algorithm. The online estimation formula is expressed as: ; in, The scalar value for the surrounding rock stiffness estimated online during the current control cycle; This is the scalar value of the surrounding rock stiffness estimated online in the previous control cycle; These are the preset first-order low-pass filter coefficients; and These are the end contact force scalars for the current control cycle and the previous control cycle, respectively; and These are the scalar values ​​representing the end position deviation between the current control cycle and the previous control cycle; To prevent extremely small constants with a denominator of zero, their values ​​are set to, for example, 10. -5 .

[0069] The adaptive impedance control module calculates and updates the virtual impedance parameters. Based on the estimated surrounding rock stiffness, the module dynamically adjusts the virtual stiffness matrix and the virtual damping matrix. In hard rock conditions, the surrounding rock stiffness increases, and the adaptive impedance control module reduces the virtual stiffness to enhance mechanical compliance and avoid impact overload; in soft rock conditions, the surrounding rock stiffness decreases, and the module increases the virtual stiffness to ensure trajectory tracking accuracy. The adaptive update formulas for virtual stiffness and virtual damping are expressed as follows: ; ; in, The virtual stiffness matrix updated for the current control cycle; The preset basic virtual stiffness matrix; This is a preset stiffness adaptive adjustment coefficient; The virtual damping matrix updated for the current control cycle; This is a preset damping ratio diagonal matrix. Since the above matrices are all positive definite diagonal matrices, the square root operation is performed independently on the diagonal elements within the matrix.

[0070] The adaptive impedance control module substitutes the updated impedance parameters into the composite control law. It then replaces the corresponding constant matrices in the basic impedance feedback control law in real time with the updated virtual stiffness and damping matrices from the current control cycle, combining these with a feedforward compensation term to output a dynamically adjusted final joint driving torque command vector. The data acquisition from the six-dimensional force sensor and the solution of the forward kinematics matrix can be implemented by those skilled in the art based on conventional robot kinematics theory; these are well-known techniques in the field and will not be elaborated upon here.

[0071] After dynamically updating the impedance parameters, the adaptive impedance control module introduces a sliding mode control mechanism to perform robust compensation in order to suppress the nonlinear dynamics of the unmodeled system and the residual error of the prediction model under unknown operating conditions. This sliding mode robust gain dynamic adjustment stage based on disturbance prediction uncertainty includes the following specific processing steps: The adaptive impedance control module constructs a proportional-differential sliding surface within the joint space. The module obtains the previously calculated joint angle error vector and joint angular velocity error vector, linearly combines them, and sets the convergence trajectory of the system state error. The calculation formula for the sliding surface is expressed as: ; in, Let the sliding surface variable vector be the vector. The matrix is ​​a pre-defined positive definite diagonal constant matrix, and the specific values ​​of its diagonal elements determine the exponential convergence speed of each joint state of the robotic arm on the sliding surface.

[0072] The adaptive impedance control module calculates the dynamic robust gain based on the prediction uncertainty. Traditional sliding mode control generally uses a fixed-boundary maximum gain, which easily leads to high-frequency current chattering at the motor physical control layer. The adaptive impedance control module extracts the Gaussian process regression results output by the online learning feedforward compensation module and introduces the variance of the prediction output as an adjustment variable into the sliding mode gain function. When the prediction variance is small, it indicates that the confidence of the prediction model is high under the current working condition, and the feedforward compensation term has offset most of the disturbance. The system lowers the sliding mode gain to reduce motor chattering. When the environment at the tunneling face changes abruptly, causing the prediction variance to increase, the system increases the sliding mode gain to provide a forced robust constraint, ensuring the closed-loop stability of the robotic arm under extreme working conditions. The formula for calculating the dynamic robust gain is expressed as follows: ; in, The dynamic sliding mode gain diagonal matrix updated for the current control cycle; The preset basic sliding mode gain diagonal matrix; This is a diagonal matrix with a preset variance scaling factor.

[0073] The adaptive impedance control module calculates the sliding mode robust compensation torque. To further smooth the control commands, the adaptive impedance control module uses a hyperbolic tangent function instead of the ideal sliding mode sign switching function, performing continuous nonlinear mapping within the boundary layer. The formula for calculating the sliding mode robust compensation torque is expressed as follows: ; in, This is the sliding mode robust compensation torque vector; The hyperbolic tangent function is used to perform element-wise hyperbolic tangent mapping on the input vector. This is a preset boundary layer smoothing factor scalar used to determine the thickness of the boundary layer.

[0074] The adaptive impedance control module updates and issues the composite control law command. The adaptive impedance control module superimposes the calculated sliding mode robust compensation torque onto the existing feedforward and impedance fusion control framework, updating the final control execution command. The mathematical expression of the updated composite control law is: ; The adaptive impedance control module uses a pre-calibrated motor torque constant to linearly map the final joint driving torque command vector into the corresponding desired current signal, and sends it as the given input of the current loop to the underlying servo driver of each joint. The mathematical derivation of the Lyapunov stability proof and sliding mode convergence boundary of the control system can be achieved by those skilled in the art based on conventional nonlinear control theory; these are well-known techniques in the field and will not be elaborated upon here.

[0075] Specific application examples: The application scenario is set as a full-face tunneling face in an underground coal mine. The tunneling equipment is a tracked cantilever tunneling machine, with a six-degree-of-freedom heavy-duty robotic arm mounted at the front of the machine body. An anchor drilling rig is installed at the end of the robotic arm to perform automatic anchoring operations on the roadway roof.

[0076] Hardware parameter configuration: An MTi-630 industrial-grade inertial measurement unit is rigidly mounted on the base of the robotic arm, with a sampling frequency set to 500Hz. An Intel RealSense depth camera and a six-dimensional torque sensor are installed at the end flange of the robotic arm, with a visual sampling frequency of 30fps. The system's main control computer runs a Linux+RTOS real-time operating system, with a control cycle set to 2ms.

[0077] Solution implementation process: During the process of the tunneling machine's cutting head cutting coal and rock, the tracked chassis is subjected to extremely strong nonlinear reaction force, which causes the robotic arm base to generate wideband random vibration with an amplitude of approximately ±15mm and a frequency between 10Hz and 45Hz.

[0078] At this point, the multi-source sensor fusion module reads data from the IMU and camera, and through time series alignment and extended Kalman filtering (EKF), removes high-frequency cutting noise and compensates for gravity components, outputting a smooth six-degree-of-freedom acceleration and velocity sequence of the base in the global reference frame.

[0079] The floating base dynamics modeling module reads the base's motion state and the encoder angles of the six joints of the robotic arm in real time. Based on the Lagrange decoupling equation, it calculates the equivalent joint disturbance torque generated by the base vibration on the second joint of the robotic arm (the shoulder joint, which is most severely affected by gravity and inertial coupling) at the current moment. It reached 120 N·m.

[0080] The online learning feedforward compensation module extracts the base vibration time-series matrix from the past 0.1 seconds (50 sampling points) and inputs it into a 1D-CNN+LSTM+GPR hybrid network. The network not only predicts the average disturbance torque for the next control cycle... The value is 118 N·m, and the GPR layer outputs the current prediction variance. The value is 0.02. Since the variance is within a low threshold, the dynamic weighting coefficients calculated by the system are... With a value close to 0.95, the system highly trusts the network prediction results and generates feedforward compensation torque commands. .

[0081] As the robotic arm's end effector presses against the tunnel roof to prepare for drilling, the surrounding rock medium transitions from soft coal seam to hard rock strata. The adaptive impedance control module detects a 300N step increase in Z-axis contact force via the end effector's six-dimensional force sensor, while the position deviation increment is only 0.5mm. Based on this, the system estimates the current equivalent stiffness of the surrounding rock online. Extremely high, triggering an impedance adaptive mechanism, resulting in a fundamental virtual stiffness matrix The diagonal parameters decrease exponentially, and the robotic arm exhibits compliant yielding characteristics in the Z-axis direction, preventing the drill pipe from breaking instantly.

[0082] Meanwhile, to address the residual 2 N·m torque deviation from feedforward compensation, the adaptive impedance control module utilizes extremely small prediction variance. Calculations show that only a very small dynamic sliding mode robustness gain needs to be applied at present. The hyperbolic tangent function tanh outputs a smooth sliding mode robust compensation torque. With resistance torque and feedforward torque The data is then merged and ultimately sent to the servo driver.

[0083] Experimental verification and effect comparison: Experimental conditions and control group setup: The experiment used a Stewart six-degree-of-freedom motion platform to simulate the broadband random vibration of the chassis during actual tunneling operations (the excitation signal originated from the spectral data collected from a real coal mine working face). The test object was a proportional six-degree-of-freedom robotic arm, and the working face was equipped with a composite variable stiffness contact wall that abruptly transitioned from low-stiffness rubber to high-stiffness steel plate.

[0084] The experiment included three control groups: Method A (Traditional Control): Employs basic pure joint space PID position closed-loop control without feedforward compensation or impedance control.

[0085] Method B (Conventional Impedance Control): Employs impedance control with fixed parameters, supplemented by static feedforward based on inverse kinematics, and features a sliding mode controller with fixed gain, without an online network prediction component.

[0086] Method C (the method of this invention): adopts a complete set of solutions provided by this invention, including dynamic decoupling, online learning prediction feedforward, adaptive impedance adjustment, and dynamic gain sliding mode control based on prediction variance.

[0087] Test 1: Free Space 3D Trajectory Tracking Error (Vibration Interference Resistance Test): Under vibration interference of ±15mm peak value and 10Hz-45Hz frequency input on the Stewart platform, the robotic arm end effector is instructed to execute a standard circular trajectory.

[0088] Data shows that the final integrated position error of method A diverges rapidly, and the system cannot be stabilized; Method B suppresses some low-frequency sloshing through static feedforward, but it has a physical lag for sudden high-frequency impacts; Method C, due to the extraction of temporal decay characteristics by the Long Short-Term Memory Network and the advance inference of the Gaussian process, ensures that the feedforward torque is sent to the motor before the physical displacement occurs, and the root mean square error (RMSE) is limited to an extremely low level, thus eliminating the transmission of chassis vibration to the end effector.

[0089] Test 2: Smoothness of Contact Force in Variable Stiffness Environments (End-of-Line Compliance Test): Set the robotic arm's end effector to apply a constant normal contact force of 500N to the contact wall surface. At 3.0s, the contact point slides from the rubber zone to the steel plate zone, and the stiffness of the surrounding rock experiences a step increase.

[0090] Data shows that method A results in a rigid collision at the instant of sudden stiffness change, with a large overshoot of the contact force and triggering overload protection. Method B produces severe torque oscillations and requires a long settling time to converge; In the initial stage of the step change in contact force, Method C uses an online stiffness estimator to quickly capture the equivalent stiffness change and dynamically lower the virtual stiffness parameter, so that the robotic arm exhibits yielding and compliant characteristics in the force axis. The contact force only produces a small overshoot and recovers smoothly to the target value in a short time.

[0091] Test 3: Verification of High-Frequency Motor Whistling Suppression (Robustness and Equipment Lifespan Assessment): The high-frequency component of the current that monitors the output torque of the servo motor on the monitoring base.

[0092] Data shows that method B, by using the maximum sliding mode gain with a fixed boundary, causes high-frequency chattering in the motor output torque. Method C is based on prediction variance The sliding mode gain is dynamically adjusted, and the gain is automatically reduced when the model prediction confidence is high, which reduces the torque chattering amplitude by an order of magnitude and reduces the mechanical wear on the mechanical reducer.

[0093] Experimental data comparison table: The quantitative statistical data of each experiment are summarized in Table 1.

[0094] Table 1. Performance comparison of different control methods in vibration compensation and contact control of tunneling manipulators

[0095] in conclusion: Combined with appendix Figure 4 The bar chart of the terminal error index shows that, under the condition of simulating broadband vibration interference, method A (traditional PID control) is unable to resist the vibration of the base due to the lack of feedforward compensation. The maximum tracking error at the end is as high as 42.51 mm, and the root mean square error (RMSE) reaches 15.63 mm. The system shows a divergent and unstable trend. While Method B (conventional impedance + static feedforward) suppressed some low-frequency errors, the maximum error was still 18.24 mm. In contrast, Method C (the method of this invention) used a depth prediction feedforward compensation mechanism to issue compensation torque in advance before interference occurred, limiting the maximum error to within 3.12 mm and the root mean square error to only 0.91 mm. Its anti-vibration interference capability and spatial positioning accuracy achieved a qualitative leap.

[0096] Combined with appendix Figure 5 The line graph of the contact force response under varying stiffness shows that when the system encounters a sudden change in contact surface stiffness (sliding from the rubber region to the steel plate region) at t=3.0s, under the command of maintaining a target contact force of 500N: Method A instantly generates a severe overshoot of over 700N, resulting in a rigid collision and triggering overload protection shutdown after 0.1 seconds; Method B, due to the difficulty of adapting the fixed impedance parameters to environmental changes, produced an overshoot of nearly 300N, accompanied by a long torque oscillation (lasting approximately 1.5 seconds). Method C (the method of this invention) benefits from the stiffness adaptive rapid adjustment mechanism, which quickly reduces the virtual stiffness at the moment of stiffness change to exhibit yielding and compliance characteristics. It not only controls the contact force overshoot to a very small amount of 18.5N, but also quickly converges and recovers to a stable state of 500N within a short 0.2 seconds, showing excellent compliance.

[0097] In summary Figure 4 Trajectory accuracy analysis, Figure 5 As can be seen from the contact compliance performance and the comparison of the data in the aforementioned table, the method provided by this invention demonstrates technical advantages in three core dimensions: trajectory tracking accuracy, contact force stability, and bottom torque output quality. This system not only overcomes the technical bottleneck of large positioning errors of robotic arms under complex vibration conditions from the root of dynamics, but also ensures safety when interacting with environments of unknown stiffness through an adaptive mechanism, fully meeting the high-precision and high-reliability operation requirements of engineering machinery in extreme downhole conditions.

Claims

1. A vibration compensation and end-effector precision positioning system for a tunneling robot arm base, characterized in that, include: The multi-source sensor fusion module is used to receive the three-axis acceleration and three-axis angular velocity of the base collected by the base inertial measurement unit and the absolute pose data obtained by the end vision sensor, perform filtering calculations and obtain the six-degree-of-freedom motion state of the base and the absolute pose of the end of the robotic arm. The floating base dynamics modeling module is used to receive the six-degree-of-freedom motion state of the base and, in combination with the physical structural parameters of the robotic arm itself, separate the equivalent joint disturbance torque caused by the base vibration. The online learning feedforward compensation module is used to receive the triaxial acceleration and triaxial angular velocity of the base as real-time base vibration signals and the equivalent joint disturbance torque, establish a disturbance prediction model and calculate the comprehensive feedforward compensation torque vector. An adaptive impedance control module is used to combine the integrated feedforward compensation torque vector and the set impedance control parameters to generate joint driving torques for driving each joint of the robotic arm.

2. The vibration compensation and end-effector precision positioning system for a tunneling robot arm base according to claim 1, characterized in that, The multi-source sensor fusion module is specifically used for: The raw acceleration data and raw angular velocity data output by the base inertial measurement unit are received, and the pure motion acceleration of the base itself is extracted as a three-axis acceleration vector through coordinate transformation and gravity compensation calculation. The extracted triaxial acceleration vectors and the original angular velocity data are subjected to low-pass filtering to output the smoothed triaxial acceleration and triaxial angular velocity of the base. Image feature extraction is performed on the image sequence acquired by the end vision sensor to calculate the absolute pose data of the robotic arm end relative to a fixed feature point in the roadway at the current moment. Time series alignment is performed on the smoothed base triaxial acceleration, triaxial angular velocity, and absolute pose data to output an aligned heterogeneous sensing data set.

3. The vibration compensation and end-effector precision positioning system for a tunneling robot arm base according to claim 2, characterized in that, The multi-source sensing fusion module is also used for: Perform extended Kalman filtering operations using the aligned heterogeneous sensor data set; Construct a system state vector that includes the base's three-dimensional position, three-dimensional velocity, three-dimensional acceleration, attitude quaternion, and three-axis angular velocity in the global reference frame; Using the base triaxial acceleration and triaxial angular velocity output from the preprocessing stage as system input, the prior state vector and prior covariance matrix at the current moment are recursively calculated; The absolute pose of the end effector is combined with the inverse matrix of the homogeneous transformation matrix of each link of the robotic arm to calculate the global pose observation of the base. The Kalman gain is calculated using the observation and the prior state vector is corrected for error. The smoothed six-degree-of-freedom displacement, velocity and acceleration data of the base are then extracted.

4. The vibration compensation and end-effector precision positioning system for a tunneling robot arm base according to claim 1, characterized in that, The floating base dynamics modeling module is specifically used for: Construct a generalized coordinate vector for the composite system of the base and the robotic arm, wherein the generalized coordinate vector includes the six-degree-of-freedom pose vector of the base and the angle vector of the actual joint of the robotic arm; Based on the generalized coordinate vector, the total kinetic energy and total potential energy of the composite system are calculated, the Lagrangian function is constructed, and a global dynamic equation is established that includes the composite system's inertial matrix, Coriolis force and centrifugal force matrices, gravity term matrix, and driving torque mapping matrix.

5. The vibration compensation and end-effector precision positioning system for a tunneling robot arm base according to claim 4, characterized in that, The floating base dynamics modeling module is also used for: The global dynamic equations are subjected to matrix partitioning and variable decoupling operations. The six-degree-of-freedom displacement, velocity and acceleration data of the base are used as known time-varying input variables, and the angular displacement, angular velocity and angular acceleration of the actual joints of the robotic arm are used as system state variables to be solved. The dynamic equations of the pure joint space are separated, and an analytical formula is generated to quantify the equivalent joint disturbance torque that generates dynamic torque impact on the joint shaft ends of the robotic arm due to the six-degree-of-freedom spatial vibration of the base.

6. The vibration compensation and end-effector precision positioning system for a tunneling robot arm base according to claim 1, characterized in that, The online learning feedforward compensation module is specifically used for: A hybrid network architecture consisting of a one-dimensional convolutional neural network, a long short-term memory network, and a Gaussian process regression layer is constructed. The one-dimensional convolutional neural network is used to extract the local abrupt change features of the real-time base vibration signal within a short time interval; The local mutation features are received through the long short-term memory network, the temporal state information of the real-time base vibration signal under a long historical period is extracted, and a high-dimensional hidden state vector is output. The high-dimensional hidden state vector is mapped to the Gaussian process regression layer to infer the predicted mean and predicted variance of the equivalent joint disturbance torque.

7. A vibration compensation and end-effector precision positioning system for a tunneling robot arm base according to claim 6, characterized in that, The online learning feedforward compensation module is also used for: An exponential decay function is constructed based on the predicted variance output by the Gaussian process regression layer. Dynamic weighting coefficients are calculated and applied to the predicted mean and the equivalent joint disturbance torque parsed in real time by the floating base dynamic modeling module. The weighted and fused outputs the comprehensive feedforward compensation torque vector. The mean of the equivalent joint disturbance torque of the prediction output is calculated in real time, and the numerical residual between the base vibration and the equivalent joint disturbance torque mapped to each joint is calculated. When the absolute value of the numerical residual exceeds the set safety error threshold within a preset number of control cycles, it is determined that the current prediction model has local degradation, and an online update operation is triggered to perform Gaussian support set reconstruction and fine-tuning of the underlying network parameters.

8. The vibration compensation and end-effector precision positioning system for a tunneling robot arm base according to claim 7, characterized in that, The adaptive impedance control module is specifically used for: The end contact force data collected by the six-dimensional force sensor at the end of the robotic arm and the end position deviation data calculated based on the joint encoder data of the robotic arm are obtained. Based on the ratio of the contact force increment to the position deviation increment, combined with a first-order low-pass filtering algorithm, the equivalent stiffness of the current contacting surrounding rock is estimated. The virtual stiffness matrix and virtual damping matrix are dynamically adjusted based on the equivalent stiffness. The updated virtual stiffness matrix and the virtual damping matrix replace the corresponding constant matrix in the preset basic impedance feedback control law in real time, and combined with the comprehensive feedforward compensation torque vector, output the dynamically adjusted final joint driving torque command vector.

9. A vibration compensation and end-effector precision positioning system for a tunneling robot arm base according to claim 8, characterized in that, The adaptive impedance control module is also used for: Extract the angle error vector and angular velocity error vector of the robotic arm joint and construct a proportional differential sliding surface by linear combination; Extract the Gaussian process regression results output by the online learning feedforward compensation module, and use the prediction variance as an adjustment variable to calculate the dynamic robust gain in the sliding mode gain function. The sliding mode robust compensation torque vector is calculated using the hyperbolic tangent function. This vector is then superimposed onto the existing feedforward and impedance fusion control framework to update the final control execution command. The final joint drive torque command vector is linearly mapped to the corresponding desired current signal, which is then sent as the given input of the current loop to the underlying servo driver of each joint.

10. A method for vibration compensation and precise end-effector positioning of a tunneling robot arm base, applied to the tunneling robot arm base vibration compensation and precise end-effector positioning system described in any one of claims 1-9, characterized in that, Includes the following steps: The base inertial measurement unit collects the triaxial acceleration and triaxial angular velocity of the base in real time, and the absolute pose data is obtained through the end vision sensor; The multi-source sensor fusion module receives the three-axis acceleration, the three-axis angular velocity and the absolute pose data of the base, performs filtering calculations and outputs the six-degree-of-freedom motion state of the base; The floating base dynamics modeling module receives the six-degree-of-freedom motion state of the base and separates the equivalent joint disturbance torque by combining it with the physical structural parameters of the robotic arm itself. The online learning feedforward compensation module receives the triaxial acceleration and triaxial angular velocity of the base as real-time base vibration signals, and receives the equivalent joint disturbance torque, establishes a disturbance prediction model, and calculates the comprehensive feedforward compensation torque vector. The adaptive impedance control module combines the integrated feedforward compensation torque vector and the set impedance control parameters to generate joint driving torques for driving each joint of the robotic arm, and outputs them to the underlying servo drivers of each joint of the robotic arm to drive the robotic arm to complete the positioning action.

Images