Multi-mode force feedback control method and system for nuclear facility robot operation

Abstract

The invention belongs to the technical field of robot teleoperation and haptic interaction, and provides a multi-mode force feedback control method and system for nuclear facility robot operation, and the method comprises the steps: collecting the state information of a master arm and a slave arm in real time, and dynamically scheduling a haptic calculation strategy according to a task state; a fast estimation strategy is enabled in the fast transit mode. Further, real environment interaction force is obtained based on dynamic model compensation and net interaction force extraction; combining the dynamically selected multi-mode force feedback parameters, and synthesizing and outputting a touch instruction to the main arm. Meanwhile, admittance correction and feed-forward compensation are carried out on a slave arm motion instruction through net interaction force and dynamic internal force, synchronous output is carried out after safety optimization, and the slave arm is controlled to move. The problems of poor task adaptability, force sense distortion and insufficient safety in nuclear facility teleoperation are solved, and intelligent, high-fidelity and safe robot teleoperation is achieved.

Description

Technical Field

[0001] This invention relates to the field of robot teleoperation and force interaction technology, and in particular to a multi-mode force feedback control method and system for operating robots in nuclear facilities. Background Technology

[0002] In the nuclear industry, in highly radioactive and hazardous work environments such as reactor internal maintenance, radioactive waste disposal, and facility decommissioning and dismantling, direct human operation is impractical, necessitating remote operation via robots. Master-slave isomorphic teleoperation robot systems, capable of remotely replicating operator movements and feeding back force information from the work environment, have become key equipment for completing such delicate and complex tasks.

[0003] However, the extremely unique working environment and diverse tasks inside nuclear facilities pose a severe challenge to existing robotic force feedback teleoperation technology: 1. Varied Task Scenarios and Requirements: Operational tasks may include rapid transfer in spacious areas, precision assembly in confined spaces, compliant handling of fragile components, and forced demolition of rigid structures. A single force feedback and control mode cannot simultaneously meet the conflicting demands of "speed," "precision," "compliance," and "force." Operators need to frequently switch between different tasks and manually adjust control parameters, resulting in a heavy workload, low efficiency, and a high risk of errors.

[0004] 2. Environmental Perception and Model Uncertainty: Nuclear facilities have complex internal structures, and long-term irradiation can lead to changes in equipment status. Traditional control methods based on pre-set precise models have poor adaptability. Factors such as robot body dynamics and joint friction can contaminate the readings of the end effector force sensors, causing the "force perception" fed back to the operator to contain interference from the robot's own movement, making it less realistic and pure, and seriously affecting the judgment of subtle contact and force states.

[0005] 3. Balancing Operational Safety and Realism: On the one hand, it is essential to ensure that the robot does not damage fragile equipment or itself in the event of accidental contact or operational errors, requiring force feedback to provide sufficient "compliance" or "warning." On the other hand, to ensure operational efficiency and accuracy, it is necessary to provide realistic and immediate force feedback in cases of high-rigidity contact. Existing systems often struggle to achieve a dynamic and intelligent balance between safety protection and operational realism.

[0006] Therefore, it is necessary to provide a multi-mode force feedback control method and system for the operation of nuclear facility robots to solve the above-mentioned technical problems. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention provides a multi-mode force feedback control method and system for operating nuclear facility robots. Through the collaborative design of dynamic scheduling of force calculation strategies, synthesis of multi-mode force feedback, and closed-loop optimization of motion commands, it effectively solves the problems of poor task adaptability, force feedback distortion, and insufficient human-machine collaboration safety in the remote operation of nuclear facility robots, thereby improving the overall operational performance of the system in complex and high-risk nuclear environments.

[0008] This invention provides a multi-mode force feedback control method for operating nuclear facility robots, applied to a master-slave isomorphic robot system including a master arm and a slave arm. The control method includes the following steps: The system collects the position and orientation information of the main arm, the six-dimensional force information of the slave arm, and the motion state information of each joint of the slave arm in real time, and generates the target motion command of the slave arm based on the position and orientation information of the main arm. Based on the real-time task status, the force calculation strategy is dynamically scheduled, wherein the dynamic scheduling means that the calculation strategy is enabled when in the fine operation mode and the estimation strategy is enabled when in the rapid transfer mode. Based on the determined force calculation strategy, the dynamic internal force of the arm is calculated using the motion state information of each joint of the arm, and the dynamic internal force is subtracted from the six-dimensional force information to obtain the net interactive force; The net interaction force is input into a preset solution model for calculation, and combined with preset force feedback mode parameters, a force feedback command is synthesized and fed back to the main arm. Based on the determined force calculation strategy, the dynamic internal force, and the net interaction force, the target motion command of the slave arm is optimized and corrected, and the optimized slave arm control command is generated and synchronously output to the slave arm servo driver, as well as the force feedback device that outputs the force feedback command to the main arm.

[0009] Preferably, the real-time acquisition of the position and orientation information of the master arm, the six-dimensional force information of the slave arm, and the motion state information of each joint of the slave arm, and the generation of the target motion command for the slave arm based on the position and orientation information of the master arm, includes: The position coordinates and attitude angle data of the end effector of the main arm are collected in real time by the main arm sensor array to form the main arm pose vector. Based on the master-slave isomorphic mapping model, the master arm pose vector is converted into the desired pose vector of the slave arm end effector. Using the arm inverse kinematics solution module, the desired pose vector is mapped to the target angle commands of each joint of the arm; By combining the joint angle and angular velocity data fed back in real time from the encoders of each joint of the arm, the target angle command is kinematically compensated and corrected to generate the target motion command of the arm.

[0010] Preferably, the step of dynamically scheduling the force perception calculation strategy based on the real-time task status includes: Acquire real-time task status information, wherein the real-time task status information includes the relative pose information between the end of the arm and the work object, and the end contact status information calculated from the six-dimensional force information of the end arm; Based on the real-time task status information, the current operation stage is determined as follows: when the distance between the end of the arm and the work object is less than the dynamic proximity threshold, or the end contact status information indicates that contact has occurred, it is determined to enter the fine operation mode; otherwise, it is determined to enter the rapid transfer mode. Based on the mode determination results, the corresponding force calculation model and calculation frequency are dynamically scheduled: when the mode is determined to be a fine operation mode, a high-precision force solver based on the complete dynamic model of the slave arm is scheduled and the first control frequency is used for calculation; when the mode is determined to be a rapid transfer mode, a rapid force estimator based on the simplified dynamic model of the slave arm is scheduled and the second control frequency lower than the first control frequency is used for estimation. The scheduled force perception calculation model and its parameters, as well as the corresponding calculation frequency, are output as the force perception calculation strategy.

[0011] Preferably, the generation of the dynamic proximity threshold includes: Based on the real-time task status information, the required level of pose accuracy for the current task, the surface features and size parameters of the work object, and the expected safe initial contact force range are extracted. Based on the required level, the surface features and size parameters of the work object, and the expected safe initial contact force range, a dynamic proximity threshold adapted to the current task scenario is calculated and generated using a preset threshold mapping rule.

[0012] Preferably, the method based on a determined force perception calculation strategy, which calculates the dynamic internal force of the arm using the motion state information of each joint of the arm, and subtracts the dynamic internal force from the six-dimensional force information to obtain the net interaction force, includes: The theoretical dynamic internal forces are calculated using real-time motion state information of each joint of the arm and the dynamic model determined according to the force perception calculation strategy. The six-dimensional force information of the arm is introduced as feedback to identify and compensate the model parameters of the dynamic model online, and generate parameter compensation amount; The theoretical dynamic internal forces are corrected using the aforementioned parameter compensation amount to obtain the compensated dynamic internal forces; The preliminary net interaction force is obtained by subtracting the compensated dynamic internal force from the real-time acquired six-dimensional force information of the slave arm, and then low-pass filtering is applied to the preliminary net interaction force to obtain the final net interaction force.

[0013] Preferably, the step of inputting the net interaction force into a preset solution model for calculation, and combining it with preset force feedback mode parameters to synthesize a force feedback command fed back to the main arm includes: The net interactive force is input into a static solution model constructed based on the principle of virtual work to calculate the basic force feedback vector, and the force feedback mode and parameters are dynamically selected according to the real-time task status information and the force perception calculation strategy. Based on the selected force feedback mode and parameters, the basic force feedback vector is modulated and dynamically adjusted to synthesize the patterned force feedback components. The patterned force feedback components are synchronized and smoothed in the time domain to generate a final force feedback command that matches the main arm servo cycle.

[0014] Preferably, the step of optimizing and correcting the target motion command of the slave arm based on the determined force calculation strategy, the dynamic internal force, and the net interaction force, generating and synchronously outputting the optimized slave arm control command to the slave arm servo driver, and the force feedback device that outputs the force feedback command to the master arm, includes: Based on the net interaction force and the dynamic internal force, the target motion command of the slave arm is subjected to admittance correction and dynamic feedforward compensation respectively to generate a pre-corrected motion expectation command; and based on the operation mode corresponding to the force calculation strategy, the motion expectation command is converted into the joint torque command of the slave arm. The joint torque command is subjected to safety limiting and smoothing processing based on the physical limits of each joint of the slave arm to generate an optimized slave arm control command. The optimized slave arm control command and the final force feedback command are time-synchronized and aligned according to the calculation frequency in the force calculation strategy, and then synchronously output to the slave arm servo driver and the main arm force feedback device, respectively.

[0015] This invention also provides a multi-mode force feedback control system for nuclear facility robot operation, used to execute a multi-mode force feedback control method for nuclear facility robot operation, including a master-slave isomorphic robot system with a master arm and a slave arm, the control system comprising: The motion mapping module is used to collect the position and orientation information of the main arm, the six-dimensional force information of the slave arm, and the motion state information of each joint of the slave arm in real time, and generate the target motion command of the slave arm based on the position and orientation information of the main arm. The strategy dynamic scheduling module is used to dynamically schedule force calculation strategies according to the real-time task status. The dynamic scheduling means that the calculation strategy is enabled when in the fine operation mode and the estimation strategy is enabled when in the fast transfer mode. The net force extraction module is used to calculate the dynamic internal force of the arm based on a determined force perception calculation strategy using the motion state information of each joint of the arm, and to subtract the dynamic internal force from the six-dimensional force information to obtain the net interaction force. The instruction synthesis module is used to input the net interaction force into a preset solution model for calculation, and combine it with preset force feedback mode parameters to synthesize a force feedback instruction fed back to the main arm. The instruction output module is used to optimize and correct the target motion command of the slave arm based on the determined force calculation strategy, the dynamic internal force and the net interaction force, generate and synchronously output the optimized slave arm control command to the slave arm servo driver, and output the force feedback command to the force feedback device of the main arm.

[0016] Compared with related technologies, the multi-mode force feedback control method and system for operating nuclear facility robots provided by this invention have the following advantages: This invention enables the system to automatically adapt to different task requirements within nuclear facilities, such as "precise operation" and "rapid transfer," through dynamic intelligent scheduling using force-sensing calculation strategies, thereby improving operational efficiency and intelligence. By acquiring high-fidelity "net interactive force" through dynamic compensation and combining it with multi-mode force feedback synthesis, it provides operators with an immersive and enhanced tactile presence, improving operational accuracy and intuitiveness. Furthermore, by constructing a closed-loop motion command optimization mechanism based on force-sensing information, it achieves compliant adaptation to the external environment and effective tracking of operational intentions, thus comprehensively enhancing the system's overall operational performance, safety, and task reliability in complex and high-risk nuclear environments. Attached Figure Description

[0017] Figure 1 A flowchart of a multi-mode force feedback control method for operating a nuclear facility robot provided by the present invention; Figure 2 This invention provides a modular structure diagram of a multi-mode force feedback control system for operating a nuclear facility robot. Detailed Implementation

[0018] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, only the parts relevant to the invention are shown in the drawings, not all structures. Moreover, unless otherwise specified, the embodiments and features described herein can be combined with each other.

[0019] It should also be noted that, for ease of description, the accompanying drawings show only the parts relevant to the invention and not all of them. Before discussing exemplary embodiments in more detail, it should be mentioned that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although the flowcharts describe the operations (or steps) as sequential processes, many of the operations can be performed in parallel, concurrently, or simultaneously. Furthermore, the order of the operations can be rearranged. The process can be terminated when its operation is completed, but it may also have additional steps not included in the drawings. The process may correspond to a method, function, procedure, subroutine, subroutine, etc.

[0020] Example 1 To facilitate understanding of the specific implementation basis of the aforementioned control method (steps S1 to S5) by those skilled in the art, the physical system components in which this method operates are described in general below. This method is applied to a complete master-slave isomorphic robot teleoperation hardware system, which mainly consists of a master operating end, a slave operating end, and a central control system. The master operating end (master arm) includes an isomorphic robotic arm body directly controlled by the operator, a master arm sensor array (such as an optical positioner and an inertial measurement unit) integrated at its end for real-time acquisition of pose information, and a force feedback device for providing force feedback to the operator.

[0021] The working end (the slave arm) is a robot that performs tasks within the nuclear environment. Its core components include the robotic arm body, encoders installed at each joint to collect motion state information, a six-dimensional force / torque sensor installed at the end to detect interactive forces, and a servo drive that receives commands and drives the motion.

[0022] All data fusion, algorithm calculation, and command scheduling are completed by the perception and central control system (i.e., the intelligent collaborative operation control and data processing system). This system integrates high-performance computing hardware and software algorithms and is responsible for executing the entire closed-loop control process from data acquisition and dynamic strategy scheduling to force calculation and synchronous output of dual-end commands.

[0023] On top of the hardware platform, the implementation of this method relies on an integrated software algorithm system and communication architecture. Its core includes: Algorithm and model library: such as master-slave isomorphic mapping model and inverse kinematics solution module for motion mapping, static solution model based on virtual work principle for force feedback calculation, and complete and simplified dynamic model library of slave arm to support dynamic scheduling strategy; Strategy scheduling and processing software: includes a force calculation strategy scheduler that performs real-time pattern determination and resource scheduling, as well as a high-precision force solver and a fast force estimator that perform the calculations. System integration and communication fundamentals: The system connects various components through a high-bandwidth, low-latency real-time communication network (such as EtherCAT) and relies on a precise time synchronization mechanism to ensure instruction synchronization; In addition, the system has a pre-configured library of force feedback mode parameters and a threshold mapping rule library for generating dynamic thresholds, and integrates necessary safety and fault-tolerance mechanisms. These software components and algorithm modules together constitute the intelligent processing core of the operation control method.

[0024] Based on the above physical components and software algorithm framework, the multi-mode force feedback control method of the present invention is executed according to the following steps, referring to... Figure 1 As shown: S1: Real-time acquisition of position and orientation information of the main arm, six-dimensional force information of the slave arm, and motion state information of each joint of the slave arm, and mapping and generating target motion commands for the slave arm based on the position and orientation information of the main arm.

[0025] Specifically, step S1 includes the following steps: S11: The position coordinates and attitude angle data of the end of the main arm are collected in real time through the main arm sensor array to form the main arm pose vector.

[0026] In this embodiment, this step is achieved at the hardware level through a high-precision optical positioning system installed at the end of the operator's handheld main arm, in collaboration with a miniature inertial measurement unit (IMU).

[0027] In practice, the optical positioning system calculates the three-dimensional position of the marker cluster at the end of the main arm in real time within its established world coordinate system at a frequency of no less than 500Hz. and posture (usually in quaternions) (This is indicated). Simultaneously, an IMU mounted close to the handle acquires triaxial acceleration at a frequency of 1 kHz. and triaxial angular velocity .

[0028] At the software level, the data fusion algorithm module of the central control system first preprocesses the IMU data (such as zero bias compensation), and then uses quaternion differential equations or direction cosine matrix integration, taking the attitude from the previous moment as the initial value, to calculate the attitude prediction based on the IMU. .

[0029] Subsequently, an Extended Kalman Filter (EKF) is used for fusion. Taking the EKF as an example, the state vector typically includes position, velocity, attitude quaternions, and sensor zero bias. The filter uses IMU data as input for the prediction step and pose data provided by the optical positioning system as input for the observation update step. Through real-time iteration, the EKF outputs the optimally estimated, smooth six-DOF pose, i.e., including the position vector. and attitude quaternions The main arm pose vector. This vector is output at a fixed period (e.g., 1ms) for use in subsequent steps.

[0030] S12: Based on the master-slave isomorphic mapping model, the master arm pose vector is converted into the desired pose vector of the slave arm end.

[0031] In this embodiment, this step is based on the master-slave isomorphic mapping model established during the system calibration phase. This model is determined during calibration, and its core is a 4×4 homogeneous transformation matrix. It describes the fixed spatial pose relationship between the master arm end-effector coordinate system and the slave arm end-effector tool coordinate system. During real-time operation, the control software receives the master arm pose vector output in step S11 and constructs it into a homogeneous transformation matrix. The mapping calculation is performed according to the following formula:

[0032] Calculated This represents the desired pose of the end effector in the slave arm's base coordinate system during the current cycle. Furthermore, the mapping model typically integrates a dynamically configurable scaling factor matrix. .For example It can be a diagonal matrix ,in This is the position scaling factor. It can be set in fine-tuning mode. In fast transfer mode, settings can be configured. .

[0033] Scaling calculations are typically performed on the position components before pose transformation, i.e. ,in This represents the original position vector of the end effector of the main arm in the base coordinate system; it is a three-dimensional column vector. Its data comes directly from the position component in the main arm pose vector output after sensor fusion in step S11. This represents the position scaling factor matrix, a 3×3 diagonal matrix used to scale the mapping from the master arm motion to the slave arm motion. Its general form is:

[0034] in These are scaling factors along the X, Y, and Z coordinate axes, respectively. These factors are dynamically configured parameters based on the real-time task status and force calculation strategy, and are the core of achieving motion scaling or motion amplification.

[0035] This represents the scaled equivalent position vector of the end effector of the main arm. This vector is used for subsequent spatial mapping to generate the desired pose vector from the end effector. The direct input location.

[0036] Ultimately, from Extracting position vectors and attitude quaternions Together, they form the desired pose vector from the end of the arm.

[0037] S13: Using the inverse kinematics solution module of the arm, the desired pose vector is mapped to the target angle command of each joint of the arm.

[0038] In this embodiment, this step is performed by the arm inverse kinematics solution module. This module receives the desired pose vector output from step S12. and Taking a typical 6-DOF serial robotic arm as an example, the module uses a numerical iterative method based on the Jacobian matrix (such as the Newton-Raphson method) for solution. The specific process is as follows: Initialization: based on the actual angles of the current joints of the arm. As the initial value for iteration ( =0).

[0039] Positive kinematics calculation: based on the joint angles of the current iteration Using the DH parameters of the slave arm, the estimated end effector pose is obtained through forward kinematics chain calculation. and their corresponding positions and posture .

[0040] Error calculation: Calculate the error between the desired pose and the current positive motion pose. Position error. Attitude error The error can be calculated using quaternions and transformed into an equivalent axis-angle error vector. (Under a small-angle approximation) Approximately 2). and Combined into a 6-dimensional terminal error vector e.

[0041] Jacobian matrix calculation and solution: Calculate the angle at the current joint. The geometric Jacobian matrix of the lower arm By solving linear equations To obtain the correction amount for the joint angle. To address ill-conditioned problems involving the Jacobian matrix near singular points, damped least squares (DLS) is typically used, i.e.:

[0042] in is the damping factor.

[0043] Iterative Update and Judgment: Update Joint Angle Estimates ,in This is the step size factor (usually ≤1). Determine the error vector. Is the norm less than a preset threshold? If so, the iteration converges, and the output is... As a target angle command from each joint of the arm If the condition is not met and the maximum number of iterations has not been exceeded, then let... Return to the positive kinematics calculation and continue iterating.

[0044] S14: Combining the joint angle and angular velocity data fed back in real time from the encoders of each joint of the arm, kinematic compensation and correction are performed on the target angle command to generate the target motion command of the arm.

[0045] In this embodiment, this step aims to compensate for the ideal theoretical joint angle command in real time, generating a target motion command that can be directly used for servo drive. The control software periodically reads the current actual joint angle from the encoders of each joint of the slave arm. and joint angular velocity The specific process is as follows: Position closed-loop control calculation: Calculate the target angle command From a practical perspective deviation Input this deviation into a digital PID position controller. The controller output is a basic joint torque feedforward, for example:

[0046] in , and It is a pre-tuned diagonal gain matrix.

[0047] Kinematic and dynamic compensation: Friction compensation: The predicted frictional torque is calculated using the Coulomb and viscous friction models. ,in and This is the diagonal matrix of the calibrated friction parameters.

[0048] Feedforward compensation: Add feedforward terms based on the derivative of the target command, such as the sum of velocity feedforward and acceleration feedforward. .

[0049] Command synthesis: All the above components are combined to generate the final joint-level control command. For example, if the underlying servo is in torque mode, the final target motion command is:

[0050] This instruction is a data packet containing the command values ​​for each joint, ready to be synchronously sent to the slave arm servo driver in the next communication cycle.

[0051] S2: Dynamically schedule force calculation strategies based on real-time task status, wherein the dynamic scheduling means enabling the calculation strategy when in fine operation mode and the estimation strategy when in rapid transfer mode.

[0052] Specifically, step S2 includes the following steps: S21: Obtain real-time task status information, wherein the real-time task status information includes the relative pose information between the end of the arm and the work object, and the end contact status information calculated from the six-dimensional force information of the end arm.

[0053] In this embodiment, firstly, real-time task status information is acquired through an integrated environmental perception system. This system utilizes binocular vision or LiDAR, and employs algorithms such as PnP or ICP to calculate the relative pose information from the end effector relative to the work object, including position difference. and posture difference And calculate the Euclidean distance between them. .

[0054] Simultaneously, the raw data from the six-dimensional force sensor at the end of the arm... The process involves comparing each axial component with a preset contact force threshold vector. The system compares and determines whether contact has occurred, and outputs end-effector contact status information (such as contact indicator and contact force vector). ).

[0055] Before step S22, the calculation of the dynamic proximity threshold is also included, and the specific calculation process is as follows: First, based on the real-time task status information, the required level of pose accuracy for the current task, the surface features and size parameters of the work object, and the expected range of safe initial contact force are extracted.

[0056] Secondly, based on the required level, the surface features and size parameters of the work object, and the expected safe initial contact force range, a dynamic proximity threshold adapted to the current task scenario is calculated and generated using a preset threshold mapping rule.

[0057] Secondly, based on the required level, the surface features and size parameters of the work object, and the expected safe initial contact force range, a dynamic proximity threshold adapted to the current task scenario is calculated and generated using a preset threshold mapping rule.

[0058] In this embodiment, before pattern determination, the system dynamically generates a dynamic proximity threshold based on the task context. Its calculation formula integrates the pose accuracy requirement level L and the radius of the object's circumscribed sphere. Surface fragility markings and safe initial contact force Parameters, for example:

[0059] Where the coefficient , , , These are pre-calibrated values.

[0060] S22: Based on the real-time task status information, determine the mode of the current operation stage: when the distance between the end of the arm and the work object is less than the dynamic proximity threshold, or the end contact status information indicates that contact has occurred, determine to enter the fine operation mode; otherwise, determine to enter the rapid transfer mode.

[0061] In this embodiment, pattern determination is performed based on the aforementioned real-time information. The determination logic is a state machine with hysteresis: if the contact flag is true, the system unconditionally enters the fine-grained operation mode; if there is no contact, the distance d is compared with a threshold. When switching from the rapid transit mode, it meets the following requirements. This triggers the switch; when switching out of the fine-grained operation mode, the following conditions must be met. ,in The hysteresis (e.g., 10 mm) is used to prevent mode oscillation.

[0062] S23: Based on the result of mode determination, dynamically schedule the corresponding force perception calculation model and calculation frequency: when it is determined to be a fine operation mode, schedule a high-precision force perception solver based on the complete dynamic model of the slave arm and use the first control frequency for calculation; when it is determined to be a rapid transfer mode, schedule a rapid force perception estimator based on the simplified dynamic model of the slave arm and use a second control frequency lower than the first control frequency for estimation.

[0063] In this embodiment, the central scheduler dynamically schedules the force perception calculation model and calculation frequency according to the determined mode. If the fine operation mode is entered, a high-precision force perception solver based on a complete dynamic model (including all mass, inertia, and friction parameters) is loaded, and a higher first control frequency (e.g., 1kHz) is assigned to it. If the rapid transfer mode is entered, a fast force perception estimator with significantly simplified computation (usually containing only gravity and main inertial compensation) is loaded, and a lower second control frequency (e.g., 500Hz) is assigned to it. The scheduling ensures smooth model switching and avoids internal state jumps.

[0064] S24: Output the scheduled force perception calculation model and its parameters, as well as the corresponding calculation frequency, as the force perception calculation strategy.

[0065] In this embodiment, the scheduler encapsulates the determined model identifier, operating parameters, and control frequency into a structured force feedback calculation strategy data packet, and writes it to shared memory or publishes it via a real-time message bus. This strategy data packet serves as a unified control command, which is read and strictly executed by subsequent modules such as S3 in each control cycle, thereby ensuring that the entire force feedback pipeline operates collaboratively under a dynamically optimized strategy.

[0066] S3: Based on the determined force calculation strategy, the dynamic internal force of the arm is calculated using the motion state information of each joint of the arm, and the dynamic internal force is subtracted from the six-dimensional force information to obtain the net interaction force.

[0067] Specifically, step S3 includes the following steps: S31: Calculate the theoretical dynamic internal force by using the real-time motion state information of each joint of the arm and the dynamic model determined according to the force perception calculation strategy.

[0068] In this embodiment, this step aims to calculate the force generated at the end of the robotic arm due to its own motion (inertia, gravity, friction, etc.), that is, excluding the "internal force" of interaction with the environment.

[0069] The specific implementation is as follows: In each control cycle, the control software reads the real-time motion status information of each joint of the arm collected in step S1, including the angle of each joint. angular velocity and angular acceleration (Angular acceleration can be estimated through angular velocity difference or state observer). At the same time, the identifier of the currently active dynamic model (full model or simplified model) and its parameter set are obtained from the force calculation strategy output in step S2.

[0070] Then, the corresponding dynamic algorithm is called to perform the calculation: If the strategy is specified as a fine-grained operation mode, a complete dynamic model is used. This model is typically built using the Newton-Euler iterative algorithm or the Lagrange method. Taking the Newton-Euler method as an example, the algorithm iterates forward from the base to the end to calculate the velocity and acceleration of each link, and then iterates backward from the end to the base to calculate the interaction forces and torques of each link. The algorithm requires dynamic parameters (such as the mass of each link). Location of the center of mass Inertial tensor Coulomb friction coefficient viscous friction coefficient The parameters have been pre-calibrated and stored in the parameter set. Finally, the algorithm outputs the result for generating the current motion, ignoring external contact forces at the end. The required theoretical six-dimensional forces / torques acting in the coordinate system at the end of the arm are denoted as theoretical dynamic internal forces. .

[0071] If the strategy is specified as rapid transport mode, a simplified dynamics model is used. This model may only contain gravity compensation terms. And coarse inertial force compensation based on the diagonal inertia matrix, for example:

[0072] in For Jacobian matrices, To simplify the inertia matrix, this model requires far less computation than the complete model.

[0073] S32: Introduce the six-dimensional force information of the slave arm as feedback to identify and compensate the model parameters of the dynamic model online, and generate parameter compensation amount.

[0074] In this embodiment, this step aims to address the problem of inaccurate or time-varying dynamic model parameters (such as load changes or friction changes) by improving model accuracy through online identification. Its implementation relies on a closed-loop parameter estimator: 1. Data Preparation: In each control cycle, collect a set of input and output data. The input is the current motion state. And the currently used dynamic model (complete or simplified). The theoretical output of the model is the result calculated in step S31. The measured corresponding quantities come from sensors: based on the principles of dynamics, the values ​​measured by the end-effector six-dimensional force sensor. When moving in free space (without contact with the environment), it should be equal to the dynamic internal force. Therefore, the system continuously monitors the end-point contact status (from step S2). When it is determined that there is no contact, the current cycle... Data pairs are considered valid samples and stored in a first-in-first-out (FIFO) data buffer.

[0075] 2. Parameter Identification: A low-priority parameter identification thread running in the background (e.g., at 100Hz) periodically checks the buffer. When valid data accumulates to a certain amount (e.g., 100 sets), this thread initiates a batch identification process. The dynamic model is represented in a linear parameterized form: ,in For the regression matrix, The vector represents the dynamic parameters to be identified (e.g., mass, inertia, friction coefficient). Using N sets of data from the buffer, overdetermined equations are constructed. The recursive least squares method or weighted least squares method is used to obtain the parameter estimates that best match the current system. .

[0076] 3. Generate compensation quantity: The parameter estimates obtained from online identification are used... Compared with the nominal values ​​of the original parameters of the model Compare and calculate parameter errors .this This is the parameter compensation amount. To prevent parameter fluctuations caused by noise interference, it can be adjusted... A low-pass filter is then applied. Finally, this compensation is passed to the next step for online model correction.

[0077] S33: The theoretical dynamic internal force is corrected using the parameter compensation amount to obtain the compensated dynamic internal force.

[0078] In this embodiment, the output of step S32 is used to "calibrate" the theoretical internal forces calculated in real time. Specifically, the implementation is as follows: In the next control cycle (or immediately after identification update), when step S31 is executed to calculate the theoretical dynamic internal forces... At that time, the original nominal parameters are no longer used directly. Instead, it uses compensated parameters. Perform dynamic calculations. Among them, It is a gain factor (0≤ ≤1), used to control the strength of compensation, can smoothly introduce parameter changes and avoid abrupt changes. Using The calculated dynamic internal forces are the compensated dynamic internal forces. This force is theoretically greater than It is closer to the actual internal forces generated when a robotic arm moves under current real physical conditions.

[0079] S34: Subtract the compensated dynamic internal force from the real-time acquired six-dimensional force information of the slave arm to obtain the preliminary net interaction force, and perform low-pass filtering on the preliminary net interaction force to obtain the final net interaction force.

[0080] In this embodiment, this step performs the core "net interaction force" extraction: 1. Subtraction Extraction: In each control cycle, read the raw data collected in real time from the end-effector six-dimensional force / torque sensor. (Preliminary hardware filtering and coordinate transformation have been performed). Subtract the compensated internal dynamic forces calculated in step S33 directly from these values. get ,this This is the initial net interaction force, which, in its theoretical physical sense, is the force generated purely from the contact or interaction between the end of the arm and the environment after eliminating the influence of the robot's own motion.

[0081] 2. Filtering: Due to factors such as sensor noise, model residual errors, and incomplete modeling of high-frequency dynamics, the initial net interaction force still contains high-frequency noise and disturbances. Therefore, filtering is necessary. Filtering is performed. Typically, a second-order low-pass Butterworth filter or a first-order low-pass filter is used. The cutoff frequency of the filter needs careful selection: too high a cutoff frequency will result in poor filtering performance, while too low a cutoff frequency will introduce phase lag, affecting the real-time performance of the feedback. For example, for fine-tuning modes (high force feedback frequency), a filter with a cutoff frequency of 50-100Hz can be used; for fast-moving modes, a lower cutoff frequency (such as 20-50Hz) can be used to more effectively suppress noise. The filtering process is performed independently on the six components of the force vector. The filtered signal is the final net interaction force.

[0082] 3. Output: The final net interaction force output is used as the core input for step S4 (force feedback command synthesis). This force vector is considered the best estimate of the actual interaction forces in the environment.

[0083] S4: The net interaction force is input into a preset solution model for calculation, and combined with preset force feedback mode parameters, a force feedback command is synthesized and fed back to the main arm.

[0084] Specifically, step S4 includes the following steps: S41: Input the net interactive force into the static solution model constructed based on the principle of virtual work, calculate the basic force feedback vector, and dynamically select the force feedback mode and parameters according to the real-time task status information and the force perception calculation strategy.

[0085] In this embodiment, this step performs two parallel tasks: calculating the basic force feedback and assigning a specific "tactile" mode to the feedback.

[0086] 1. Calculation of basic force feedback vector: The input is the final net interaction force vector output from step S3. It is a 6-dimensional vector containing three force components and three torque components. This vector is input into a statics solution model based on the principle of virtual work. The core function of this model is to map the interaction forces / torques in the tool coordinate system of the boom end to the equivalent forces / torques in the handle coordinate system of the boom operator end. Its mathematical model is usually represented as a coordinate transformation: ,in, It is a 6×6 adjoint transformation matrix that encapsulates the rotation matrix from the end-effector tool coordinate system to the main arm handle coordinate system. (Used to change the direction of force and torque). It is the geometric Jacobian matrix (or its translational portion) of the main arm operating end in the current pose. This represents its inverse transpose, used to equivalently map the torque at the end to the operator's hand. The calculated... This is the basic force feedback vector, which theoretically reflects the performance of environmental interaction forces on the main operating end directly and without modification.

[0087] 2. Dynamic Selection of Force Feedback Mode and Parameters: Simultaneously, based on real-time task status information (such as distance d and contact state in step S2) and force calculation strategy (whether it's fine-tuning or rapid transfer mode), the system dynamically selects the most suitable force feedback mode from a predefined force feedback mode parameter library. The parameter library typically exists in the form of a lookup table or rule set, for example: When in rapid transfer mode and without contact, select "low-resistance guidance mode". Its parameter set may include a global force scaling factor (e.g., 0.3) and a virtual damping coefficient.

[0088] When in fine-tuning mode and already in contact, select "Interactive Force Reproduction Mode". Its parameters are designed to ensure force fidelity, with a scaling factor of approximately 1, and may include a filter cutoff frequency to prevent high-frequency jitter.

[0089] When the magnitude or rate of change of the net interaction force exceeds a certain safety threshold, or when the system detects abnormal vibration, the "Vibration Warning Mode" is selected. Its parameters include vibration frequency, amplitude, and waveform (e.g., a sine wave). The selection result is a mode identifier and its associated parameter structure, which will be used in the next step of modulating the fundamental vector.

[0090] S42: Based on the selected force feedback mode and parameters, the basic force feedback vector is modulated and dynamically adjusted to synthesize the modulated force feedback components.

[0091] In this embodiment, this step is central to giving force a specific "feeling." It receives the basic force feedback vector. The selected mode parameters are used to output the modulated force vector.

[0092] Low-drag guidance mode modulation: This mode is designed to reduce operational drag during free movement, providing a smooth feel. The modulation algorithm is as follows: .in, It is the instantaneous velocity vector at the main arm operating end (obtained by data difference in step S1). This formula reduces environmental force feedback ( <1) At the same time, an additional viscous damping force is applied in the opposite direction to the movement of the handle. This makes movement feel more stable and easier to control, but not completely powerless.

[0093] Interactive reproduction mode modulation: This mode pursues high fidelity. The modulation mainly involves: ,generally The value is 1. However, to ensure stability and suppress potential noise, it will affect... Apply a low-pass filter with a small phase lag (such as a second-order Butterworth filter with a cutoff frequency of 30Hz) to remove high-frequency components that may cause hand discomfort while maintaining the realism of force perception.

[0094] Vibration warning mode modulation: This mode is designed to provide tactile alerts. The modulation algorithm is as follows: .in, It is a superimposed vibration vector. For example, a sinusoidal vibration force can be generated along a sensitive axis (such as the Z-axis) of the main boom handle: Vibration amplitude and frequency Adjustable based on alarm level. (Synthesized) This refers to the patterned force feedback component, which includes both basic interactive force information and specific guiding or warning tactile sensations.

[0095] S43: Synchronize and smooth the patterned force feedback components in the time domain to generate the final force feedback command that matches the main arm servo cycle.

[0096] In this embodiment, this step performs post-processing on the modulated force to ensure the stability, safety, and synchronization with other parts of the system of the output command.

[0097] 1. Synchronous processing: Modulated force feedback components This might be calculated at a frequency specified by the force feedback calculation strategy (e.g., 1kHz in fine mode, 500Hz in fast mode). However, the force feedback device of the main arm (usually a motor) has its own servo control cycle (e.g., 1kHz). Therefore, it is necessary to... The resampling or hold mechanism synchronizes the system to the main arm servo control cycle. For example, if the force calculation frequency is 500Hz and the servo cycle is 1kHz, then each time a new force is calculated... This value will be output for the next two servo cycles, until the next one. It was calculated.

[0098] 2. Time-domain smoothing: To avoid step changes or jitter in the output force caused by mode switching, parameter jumps, or computational noise, the synchronized force command needs to be smoothed by filtering. A first-order low-pass filter or a moving average filter is typically used. This step effectively eliminates high-frequency jitter in the command, protects the force feedback motor, and provides the operator with a smooth tactile feedback.

[0099] 3. Safety Limit: Before outputting, the smoothed force vector must be... Each component is limited to ensure it does not exceed the maximum continuous output torque and peak torque of the main boom force feedback device motor, preventing equipment overload damage. The limiting value is preset according to the motor specifications.

[0100] 4. Generate final command: Force vector after synchronization, smoothing, and amplitude limiting. The data is encapsulated into a data frame conforming to the communication protocol of the main arm force feedback device and tagged with a precise timestamp. This data packet is the final force feedback command, which will be sent precisely to the force feedback device of the main arm in the next servo cycle, driving the motor to generate the corresponding force / torque, which is ultimately applied to the operator's hand, completing the closed loop from environmental interaction force to operator tactile sensation.

[0101] S5: Based on the determined force calculation strategy, the dynamic internal force and the net interaction force, optimize and correct the target motion command of the slave arm, generate and synchronously output the optimized slave arm control command to the slave arm servo driver, and output the force feedback command to the force feedback device of the main arm.

[0102] Specifically, step S5 includes the following steps: S51: Based on the net interaction force and the dynamic internal force, admittance correction and dynamic feedforward compensation are performed on the target motion command of the slave arm respectively to generate a preliminarily corrected motion expectation command; and based on the operation mode corresponding to the force calculation strategy, the motion expectation command is converted into a joint torque command of the slave arm.

[0103] In this embodiment, this step receives various information from upstream steps and comprehensively optimizes and transforms the original slave arm target motion command. The specific implementation is as follows: The control software receives the original target motion command generated in step S1 (usually the target position vector and target velocity vector in the joint space), the compensated internal dynamic force vector and the final net interaction force vector calculated in step S3, and the force perception calculation strategy determined in step S2 (containing the current operation mode).

[0104] 1. Admittance Correction: This correction aims to ensure a compliant response of the slave arm to external contact. The system pre-defines an admittance model, typically a second-order system consisting of desired inertia diagonal matrices, desired damping diagonal matrices, and desired stiffness diagonal matrices. This model establishes the relationship between the contact force and the resulting end-effector position correction, velocity correction, and acceleration correction. In each control cycle, the net interaction force vector is substituted into this model, and the end-effector position correction vector in Cartesian space (i.e., the three-dimensional space of the robot's end effector) is calculated in real time through numerical integration. Subsequently, the displacement correction vector in Cartesian space is mapped to the angle correction vector in joint space using the inverse matrix of the slave arm's Jacobian matrix (which establishes the differential relationship between joint motion and end-effector motion).

[0105] 2. Dynamic Feedforward Compensation: This compensation aims to counteract the influence of the robot's own dynamics using an accurate model, thereby improving tracking performance. The same dynamic model (such as the Newton-Euler algorithm) upon which the compensated internal forces depend is run again, but this time the input joint motion commands are the uncorrected original target commands, including the target angle, target angular velocity, and target angular acceleration. The algorithm calculates the full joint torque vector theoretically required for accurate tracking of this command; this is the feedforward compensation torque vector.

[0106] 3. Generate motion expectation instructions and conversions: First, a preliminary corrected joint position expectation vector is generated, which is the original target joint angle vector plus the joint angle correction vector obtained by admittance correction.

[0107] Then, based on the operating mode corresponding to the force calculation strategy, a control law is selected to convert the motion expectation into joint torque commands: For fine-tuning operation, position-based impedance control is employed. The algorithm uses the corrected joint position expectation vector as the position target and the current actual joint position vector of the arm as feedback to calculate the position error vector. This error vector passes through a virtual impedance controller (typically a proportional-derivative controller with parameters being the proportional gain matrix and the derivative gain matrix) to generate a basic corrective torque vector. The final joint torque pre-command is the corrective torque vector plus the feedforward compensation torque vector.

[0108] In the rapid transfer mode, computational torque control is employed. This algorithm is primarily based on feedforward compensation, aiming to accurately track the trajectory. Its joint torque pre-command is directly equal to the feedforward compensation torque vector. In this mode, the angle correction generated by admittance correction is usually ignored or given minimal weight, because this mode prioritizes the speed and accuracy of motion tracking rather than compliance with external forces.

[0109] S52: Based on the physical limits of each joint of the slave arm, the joint torque command is subjected to safety limiting and smoothing processing to generate an optimized slave arm control command.

[0110] In this embodiment, this step ensures that the instructions sent to the driver are safe and feasible, preventing device damage or sudden changes in motion.

[0111] Safety Limiting Processing: The input is the unprocessed joint torque pre-command vector generated in step S51. The system pre-stores the physical limit parameters of each joint of the arm, including the maximum continuous output torque vector and the maximum peak torque vector. First, peak torque is limited: for each joint's torque command, if its absolute value exceeds the peak torque limit of that joint, it is clipped to the limit range while maintaining its original sign. Next, continuous torque and power are limited: the system continuously monitors the torque output history of each joint and calculates its root mean square value within a time window. If the root mean square torque value of a joint is close to its continuous torque limit, the current command for that joint is dynamically attenuated. At the same time, combined with the current joint's actual angular velocity vector, it ensures that the instantaneous mechanical power does not exceed the allowable values ​​of the motor and reducer.

[0112] Smoothing: To avoid command jumps caused by mode switching, abrupt admittance correction changes, or limiting, the limited torque command is smoothed by filtering. A first-order low-pass filter is typically used. The filtered output command value for the current cycle is equal to a smoothing factor (between 0 and 1) multiplied by the current cycle's limited input command value, plus the difference (minus the smoothing factor) multiplied by the output command value of the previous cycle's filter. The smoothing factor is selected based on the current operating mode; a smaller value is used in fine-tuning mode for smoother force feedback, while a larger value is used in fast-transfer mode to ensure response speed. The command vector after limiting and smoothing is the optimized slave arm control command.

[0113] S53: The optimized slave arm control command and the final force feedback command are time-synchronized and aligned according to the calculation frequency in the force calculation strategy, and then synchronously output to the slave arm servo driver and the main arm force feedback device, respectively.

[0114] In this embodiment, this step is the final output stage of the control loop, ensuring a high degree of coordination between the master and slave ends.

[0115] Time Synchronization Alignment: The system maintains a high-precision global clock. Both the final force feedback command vector generated in step S4 and the optimized slave arm control command vector generated in step S52 carry a timestamp indicating the completion of their calculations. The output scheduler calculates a common, precise future output time point based on the control frequency (first control frequency or second control frequency) specified in the force calculation strategy and in conjunction with the communication cycle of the master and slave arm servo drives. This time point must ensure that: the slave arm control command is received by the drive at the start of its corresponding servo cycle; simultaneously, the application time of the force feedback command and the expected moment when the slave arm exerts a corresponding force on the environment are as synchronized as possible with the operator's perception to maintain consistency between master and slave operations.

[0116] Synchronous Output: At a preset output time, the output scheduler synchronously triggers two sets of data transmission operations via a real-time communication network: The optimized slave arm control command data packet is encapsulated according to the communication protocol supported by the slave arm servo driver and synchronously sent to the slave arm servo driver; simultaneously, the final force feedback command data packet is encapsulated according to the control protocol of the master arm force feedback device and synchronously sent to the master arm force feedback device. This precise scheduling and synchronous output mechanism based on global timestamps minimizes the perception delay between master-end force sensing and slave-end motion, ensuring immersive and real-time operation during teleoperation.

[0117] Example 2 This invention also provides a multi-mode force feedback control system for nuclear facility robot operation, used to execute a multi-mode force feedback control method for nuclear facility robot operation, including a master-slave isomorphic robot system with a master arm and a slave arm, referenced. Figure 2 As shown, the control system includes: The motion mapping module 100 is used to collect the position and orientation information of the main arm, the six-dimensional force information of the slave arm, and the motion state information of each joint of the slave arm in real time, and generate the target motion command of the slave arm based on the position and orientation information of the main arm.

[0118] The strategy dynamic scheduling module 200 is used to dynamically schedule force calculation strategies according to the real-time task status, wherein the dynamic scheduling is to enable the calculation strategy when in the fine operation mode and the estimation strategy when in the fast transfer mode.

[0119] The net force extraction module 300 is used to calculate the dynamic internal force of the arm based on a determined force calculation strategy using the motion state information of each joint of the arm, and to subtract the dynamic internal force from the six-dimensional force information to obtain the net interaction force.

[0120] The instruction synthesis module 400 is used to input the net interaction force into a preset solution model for calculation, and combine it with preset force feedback mode parameters to synthesize a force feedback instruction that is fed back to the main arm.

[0121] The instruction output module 500 is used to optimize and correct the target motion command of the slave arm based on the determined force calculation strategy, the dynamic internal force and the net interaction force, generate and synchronously output the optimized slave arm control command to the slave arm servo driver, and output the force feedback command to the force feedback device of the main arm.

[0122] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0123] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, including read-only memory (ROM), random access memory (RAM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), one-time programmable read-only memory (OTPROM), electrically erasable programmable read-only memory (EEPROM), compact disc read-only memory (CD-ROM) or other optical disc storage, disk storage, magnetic tape storage, or any other computer-readable medium capable of carrying or storing data.

[0124] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

Claims

1. A multi-mode force feedback control method for operating a nuclear facility robot, applied to a master-slave isomorphic robot system including a master arm and a slave arm, characterized in that, The control method includes the following steps: The system collects the position and orientation information of the main arm, the six-dimensional force information of the slave arm, and the motion state information of each joint of the slave arm in real time, and generates the target motion command of the slave arm based on the position and orientation information of the main arm. Based on the real-time task status, the force calculation strategy is dynamically scheduled, wherein the dynamic scheduling means that the calculation strategy is enabled when in the fine operation mode and the estimation strategy is enabled when in the rapid transfer mode. Based on the determined force calculation strategy, the dynamic internal force of the arm is calculated using the motion state information of each joint of the arm, and the dynamic internal force is subtracted from the six-dimensional force information to obtain the net interactive force. The net interaction force is input into a preset solution model for calculation, and combined with preset force feedback mode parameters, a force feedback command is synthesized and fed back to the main arm. Based on the determined force calculation strategy, the dynamic internal force, and the net interaction force, the target motion command of the slave arm is optimized and corrected, and the optimized slave arm control command is generated and synchronously output to the slave arm servo driver, and the force feedback command is output to the force feedback device of the master arm.

2. The multi-mode force feedback control method for operating a nuclear facility robot according to claim 1, characterized in that, The real-time acquisition of the position and orientation information of the master arm, the six-dimensional force information of the slave arm, and the motion state information of each joint of the slave arm, and the mapping and generation of the target motion command of the slave arm based on the position and orientation information of the master arm, includes: The position coordinates and attitude angle data of the end effector of the main arm are collected in real time by the main arm sensor array to form the main arm pose vector. Based on the master-slave isomorphic mapping model, the master arm pose vector is converted into the desired pose vector of the slave arm end effector. Using the arm inverse kinematics solution module, the desired pose vector is mapped to the target angle commands of each joint of the arm; By combining the joint angle and angular velocity data fed back in real time from the encoders of each joint of the arm, the target angle command is kinematically compensated and corrected to generate the target motion command of the arm.

3. The multi-mode force feedback control method for operating a nuclear facility robot according to claim 2, characterized in that, The dynamic scheduling of force perception calculation strategies based on real-time task status includes: Acquire real-time task status information, wherein the real-time task status information includes the relative pose information between the end of the arm and the work object, and the end contact status information calculated from the six-dimensional force information of the end arm; Based on the real-time task status information, the current operation stage is determined as follows: when the distance between the end of the arm and the work object is less than the dynamic proximity threshold, or the end contact status information indicates that contact has occurred, it is determined to enter the fine operation mode; otherwise, it is determined to enter the rapid transfer mode. Based on the mode determination results, the corresponding force calculation model and calculation frequency are dynamically scheduled: when the mode is determined to be a fine operation mode, a high-precision force solver based on the complete dynamic model of the slave arm is scheduled and the first control frequency is used for calculation; when the mode is determined to be a rapid transfer mode, a rapid force estimator based on the simplified dynamic model of the slave arm is scheduled and the second control frequency lower than the first control frequency is used for estimation. The scheduled force perception calculation model and its parameters, as well as the corresponding calculation frequency, are output as the force perception calculation strategy.

4. The multi-mode force feedback control method for operating a nuclear facility robot according to claim 3, characterized in that, The generation of the dynamic proximity threshold includes: Based on the real-time task status information, the required level of pose accuracy for the current task, the surface features and size parameters of the work object, and the expected safe initial contact force range are extracted. Based on the required level, the surface features and size parameters of the work object, and the expected safe initial contact force range, a dynamic proximity threshold adapted to the current task scenario is calculated and generated using a preset threshold mapping rule.

5. The multi-mode force feedback control method for operating a nuclear facility robot according to claim 4, characterized in that, The defined force calculation strategy utilizes the motion state information of each joint of the arm to calculate the dynamic internal force of the arm, and subtracts the dynamic internal force from the six-dimensional force information to obtain the net interaction force, including: The theoretical dynamic internal forces are calculated using real-time motion state information of each joint of the arm and the dynamic model determined according to the force perception calculation strategy. The six-dimensional force information of the arm is introduced as feedback to identify and compensate the model parameters of the dynamic model online, and generate parameter compensation amount; The theoretical dynamic internal forces are corrected using the aforementioned parameter compensation amount to obtain the compensated dynamic internal forces; The preliminary net interaction force is obtained by subtracting the compensated dynamic internal force from the real-time acquired six-dimensional force information of the slave arm, and then low-pass filtering is applied to the preliminary net interaction force to obtain the final net interaction force.

6. The multi-mode force feedback control method for operating a nuclear facility robot according to claim 5, characterized in that, The step of inputting the net interaction force into a preset solution model for calculation, and combining it with preset force feedback mode parameters to synthesize a force feedback command fed back to the main arm includes: The net interactive force is input into a static solution model constructed based on the principle of virtual work to calculate the basic force feedback vector, and the force feedback mode and parameters are dynamically selected according to the real-time task status information and the force perception calculation strategy. Based on the selected force feedback mode and parameters, the basic force feedback vector is modulated and dynamically adjusted to synthesize the patterned force feedback components. The patterned force feedback components are synchronized and smoothed in the time domain to generate a final force feedback command that matches the main arm servo cycle.

7. A multi-mode force feedback control method for operating a nuclear facility robot according to claim 6, characterized in that, The method for optimizing and correcting the target motion command of the slave arm based on the determined force calculation strategy, the dynamic internal force, and the net interaction force, generating and synchronously outputting the optimized slave arm control command to the slave arm servo driver, and a force feedback device for outputting the force feedback command to the master arm, includes: Based on the net interaction force and the dynamic internal force, the target motion command of the slave arm is subjected to admittance correction and dynamic feedforward compensation respectively to generate a pre-corrected motion expectation command; and based on the operation mode corresponding to the force calculation strategy, the motion expectation command is converted into the joint torque command of the slave arm. The joint torque command is subjected to safety limiting and smoothing processing based on the physical limits of each joint of the slave arm to generate an optimized slave arm control command. The optimized slave arm control command and the final force feedback command are time-synchronized and aligned according to the calculation frequency in the force calculation strategy, and then synchronously output to the slave arm servo driver and the main arm force feedback device, respectively.

8. A multi-mode force feedback control system for operating a nuclear facility robot, used to execute a multi-mode force feedback control method for operating a nuclear facility robot as described in any one of claims 1 to 7, comprising a master-slave isomorphic robot system with a master arm and a slave arm, characterized in that, The control system includes: The motion mapping module is used to collect the position and orientation information of the main arm, the six-dimensional force information of the slave arm, and the motion state information of each joint of the slave arm in real time, and generate the target motion command of the slave arm based on the position and orientation information of the main arm. The strategy dynamic scheduling module is used to dynamically schedule force calculation strategies according to the real-time task status. The dynamic scheduling means that the calculation strategy is enabled when in the fine operation mode and the estimation strategy is enabled when in the fast transfer mode. The net force extraction module is used to calculate the dynamic internal force of the arm based on a determined force perception calculation strategy using the motion state information of each joint of the arm, and to subtract the dynamic internal force from the six-dimensional force information to obtain the net interaction force. The instruction synthesis module is used to input the net interaction force into a preset solution model for calculation, and combine it with preset force feedback mode parameters to synthesize a force feedback instruction fed back to the main arm. The instruction output module is used to optimize and correct the target motion command of the slave arm based on the determined force calculation strategy, the dynamic internal force and the net interaction force, generate and synchronously output the optimized slave arm control command to the slave arm servo driver, and output the force feedback command to the force feedback device of the main arm.

Images