Three-arm robot control method and system suitable for multiple types of operation of switch cabinet

Abstract

The application discloses a kind of three-arm robot control method and system suitable for switch cabinet multi-type operation, the control method includes: driving rear monitoring arm to carry out multi-view scanning to switch cabinet operation area, constructs the prior static three-dimensional grid of operation environment;According to task attribute matching heterogeneous operating arm, ensure that operation chain is started under monitoring stable;In the process of executing normal contact operation, the real-time lifting displacement of lifting mechanism is acquired in real time, the dynamic safety euclidean distance of the end of first operating arm in front or the end of second operating arm in front from the nearest obstacle in switch cabinet environment is solved;Extract pure physical external disturbance torque residual, through the fusion dynamic safety euclidean distance, the residual is modulated by cross-modal risk index;When the cross-modal collision risk index is out of limit, the nominal normal contact operation trajectory of operating arm in front is interrupted, and according to the heterogeneous attribute of the end of operating arm triggered risk, adaptive execution asymmetric control is carried out, and cross-modal monitoring and closed-loop control are realized.

Description

Technical Field

[0001] This invention belongs to the field of intelligent operation and maintenance technology of power systems, and more specifically, relates to a three-arm robot control method and system applicable to various types of switchgear operations. Background Technology

[0002] Switchgear is a key piece of equipment in a power distribution network that directly faces users. It integrates circuit breakers, disconnectors, handcarts, and various control and indicating components. To ensure power supply reliability and personnel safety, operation and maintenance procedures require periodic switching operations, status verification, contact temperature measurement, and anomaly handling for switchgear. Typical operational tasks include: jogging the opening and closing buttons, rotating the mode selection knob to multiple positions, non-contact infrared temperature measurement of critical areas such as cable compartments and busbar compartments, precise docking and screwing in / out of the handcart crank interface, and real-time video monitoring and status identification covering the entire process. These operational objects have significantly different physical properties: buttons have short travel and low rebound force; knobs have clear position positioning; the handcart crank interface requires alignment to transmit large torque; and temperature measurement tasks require a stable measurement optical path and a specified distance from the surface being measured. Meanwhile, various components on the switchgear panel are discretely distributed along the height of the cabinet, and the operating points cover a wide range; the front working passage usually only accommodates one person, and adjacent cabinets are arranged in rows, with dense equipment and close electrical intervals. There is an urgent need for a robot system that can adapt to complex working conditions and realize multi-task parallel processing to replace manual labor in high-risk operations and improve the intelligent operation and maintenance level of the power system.

[0003] Currently, the mainstream technical solutions for automated operation robots in substations or switchgear scenarios mainly adopt a configuration of a mobile chassis combined with a single-degree-of-freedom robotic arm, or integrate various sensors, such as vision cameras and infrared temperature measurement modules, directly into the end effector or near the arm body. In this type of solution, the mobile chassis is responsible for moving the robot to the designated working position in front of the target switchgear, and then the single robotic arm executes specific operations according to a preset path or remote commands. To achieve environmental perception and operation guidance, a vision sensor is usually installed at the end effector of the robotic arm, or a wide-angle camera is set at a fixed position on the robot body to locate and identify the operation target on the switchgear using visual recognition technology. This single-arm architecture with integrated sensors is considered a preliminary solution for realizing automated operation of switchgear due to its relatively compact mechanical structure and simple control system, and has been initially applied in some scenarios.

[0004] While the aforementioned existing technologies have achieved automation of switchgear operation to some extent, they still cannot effectively solve existing problems when facing typical application scenarios such as switchgear, where targets are highly dispersed, operation objects are diverse, operating space is narrow, and safety requirements are extremely high. For example, a single robotic arm needs to simultaneously perform three types of tasks: close-range precision operation, multi-angle visual perception, and real-time adjustment of the body posture, leading to competition among different tasks for limited degrees of freedom and field of view resources. Secondly, fixed cameras or wrist cameras are prone to field of view obstruction after the robotic arm approaches the operation target, making it impossible to continuously and completely monitor the relative relationship between the end effector and the target object. In addition, when facing operation objects with significant differences in shape, size, rigidity, and mechanical properties, such as buttons, knobs, cabinet door handles, and infrared temperature measuring points, as well as significant height variations, it is difficult for a single type of end effector and a single robotic arm to simultaneously achieve dexterity, structural rigidity, and spatial coverage. Summary of the Invention

[0005] In view of the above-mentioned defects or improvement needs of the existing technology, the present invention provides a three-arm robot control method and system applicable to various types of switchgear operations. By dividing the work among the three arms, matching heterogeneous end effectors, lifting compensation and cross-modal risk modulation, the three core problems of task resource conflict, field of view occlusion and insufficient end effector adaptability in the existing technology are systematically solved, and the stability, safety and intelligence level of various types of switchgear operations are significantly improved.

[0006] To achieve the above objectives, the present invention provides a three-arm robot control method applicable to various types of switchgear operations, which is applied to a three-arm robot system. The three-arm robot system includes a mobile platform, a lifting mechanism installed in the front half of the mobile platform, a front first operating arm and a front second operating arm installed on the lifting mechanism, and a rear monitoring arm installed in the rear half of the mobile platform and equipped with a depth vision perception component. The front first operating arm and the front second operating arm are configured with heterogeneous functions, and are respectively equipped with a dexterous operating end effector and a dedicated working end effector. The control method includes the following steps: S1: After the driving mobile platform moves to the preset work station, the driving rear monitoring arm performs multi-view scanning of the switch cabinet work area, constructs a priori static three-dimensional mesh of the work environment, and confirms the initial standby pose of the front first operating arm and the front second operating arm. S2: Match heterogeneous manipulators according to task attributes, calculate the nominal driving torque covering gravity and inertia compensation and send it out in real time, guide the dexterous end effector of the first manipulator or the dedicated working end effector of the second manipulator to perform normal contact actions, and ensure that the work chain starts stably under monitoring. S3: During normal contact operation, the real-time lifting displacement of the lifting mechanism is acquired in real time, and the relative coordinate offset between the rear monitoring arm and the front operating arm base caused by the lifting motion is compensated by the homogeneous transformation matrix. Then, the dynamic safe Euclidean distance between the end of the front first operating arm or the end of the front second operating arm and the nearest obstacle in the switch cabinet environment is calculated. S4: The parallel drive's built-in generalized momentum observer eliminates interference from the lifting mechanism to extract the residual of the pure physical external disturbance torque; and modulates the residual with a cross-modal risk index by fusing dynamic safety Euclidean distance; when the cross-modal collision risk index exceeds the limit, the nominal normal contact operation trajectory of the front operating arm is interrupted, and asymmetric control is adaptively executed according to the heterogeneous attributes of the operating arm end that trigger the risk, so as to realize cross-modal monitoring and closed-loop control.

[0007] Further, step S1 includes: Step S11: Drive the mobile platform to achieve autonomous navigation using vehicle-mounted lidar and odometer; after arriving at the preset work station, level the chassis through ground support stiffness feedback to ensure that the bases of the first front operating arm and the second front operating arm are on the horizontal reference plane. Step S12: By using the depth sensing component carried by the rear monitoring arm to avoid the physical interference path of the front operating arm and perform surround visual sampling, a priori static three-dimensional mesh of the switch cabinet surface is constructed by projecting multiple frames of depth images onto the coordinate system of the mobile platform base. Step S13: Load the constructed prior static 3D mesh into the digital twin engine, calculate the collision-free standby path of the first front manipulator and the second front manipulator in the current environment, drive the two manipulators to the initial working pose through joint angle closed-loop control, and use visual feedback to perform online alignment correction between the physical pose and the virtual model.

[0008] Further, step S2 includes: Step S21: Divide the task to be executed into fine interaction tasks or positioning and guidance tasks; if the task involves pressing a button or turning a knob, activate the first front operating arm equipped with a dexterous end effector; if the task involves infrared temperature measurement or standard interface docking, activate the second front operating arm equipped with a dedicated operating end effector. Step S22: Within the established prior static 3D mesh, generate the desired working path of the end effector based on the target object position, and use the inverse kinematics algorithm to convert the desired working path into a sequence of target angles, target angular velocities, and target accelerations for each joint motor. Step S23: In real time, the current height of the lifting mechanism and the position of the manipulator are integrated, and the total torque required to maintain the stable movement of the robotic arm is calculated through positive dynamic compensation and sent to the servo drives of each joint.

[0009] Further, step S3 includes: Step S31: The real-time lifting displacement data of the lifting mechanism relative to the mobile platform body in the vertical direction is obtained in real time by the displacement sensor installed at the drive end of the lifting mechanism. Step S32: Construct a time-varying translation transformation matrix based on real-time lifting displacement data, and combine it with the geometric installation relationship between the rear monitoring arm and the front first operating arm or the front second operating arm base to correct the relative spatial mapping relationship between the two in real time. Step S33: Substitute the compensated spatial pose into the digital twin engine, and obtain the dynamic safe Euclidean distance by calculating the geometric relationship between the real-time coordinates of the end effector of the first or second front manipulator in virtual space and the obstacle vertices in the prior static 3D mesh. The dynamic safe Euclidean distance is calculated as follows: ; In the formula, For a moment The dynamic safe Euclidean distance; The real-time three-dimensional spatial coordinates of the end effector of the first or second front manipulator in the coordinate system of the mobile platform base; The set of obstacle vertices in the prior static 3D mesh constructed by the rear monitoring arm.

[0010] Further, step S4 includes: Step S41: Drive the built-in generalized momentum observer, inject the gravity compensation term and flutter energy absorption term related to the lifting mechanism in real time into the dynamic integration stage, and extract the pure physical external disturbance torque residual after removing the time-varying base interference by solving the integral deviation between the nominal momentum and the measured momentum over the historical time interval. Step S42: Keeping the physical force control characteristics of the generalized momentum observer unchanged, the residual of the pure physical external disturbance torque is fused using the dynamic safety Euclidean distance to construct a cross-modal collision risk index; Step S43: Perform a high-frequency comparison between the absolute values ​​of each joint component of the cross-modal collision risk index and the preset absolute safety threshold column vector; when any component in the cross-modal collision risk index exceeds the corresponding threshold in the absolute safety threshold column vector, forcibly freeze the nominal normal contact operation trajectory issued to the front first manipulator or the front second manipulator, and at the same time, extract the collision event data packet to determine the type of end effector mounted on the manipulator that triggered the risk; Step S44: Execute an asymmetric response mechanism based on the extracted end effector type. If the risk is triggered by an operating arm equipped with a dedicated end effector, switch to torque control and activate the tension negative feedback correction mode. If the risk is triggered by an operating arm equipped with a dexterous operating end effector, immediately execute a zero stiffness resistance release action. Set the target driving torque of each joint servo driver of the operating arm to the sum of the real-time gravity compensation torque and the internal friction compensation of the system, so that the operating arm presents a passive retraction state of spring unloading in the force direction.

[0011] Furthermore, the solution method for the residual of the purely physical external disturbance torque is as follows: ; In the formula, , Each represents the current time. With historical moments The column vector of residual external perturbation torques extracted by the generalized momentum observer; This represents the current maximum actual runtime. For historical time dummy scalar variables within the integral operator; The observation gain matrix is ​​a diagonal constant. , At the current moment, either the first or second front manipulator is in position. The actual generalized momentum vector is zero at the initial moment; To be at a historical moment The nominal driving torque column vector is sent to each joint motor of the corresponding manipulator arm; To be at a historical moment The transpose of the Coriolis force and centrifugal force matrix corresponding to the manipulator; To be at a historical moment The time-varying gravity compensation torque vector; This is the physical mapping column vector from helical vibration to the joint torque space; To be at a historical moment Scalar vibration velocity of the lifting mechanism actuator.

[0012] Furthermore, the modulation method of the cross-modal collision risk index is as follows: ; In the formula, For a moment The collision risk index vector after cross-modal fusion; It is a unit diagonal matrix with the same dimension as the joint degrees of freedom of the first or second front manipulator. This is the diagonal matrix for visual sensitization gain; The space sensitivity decay constant; For a moment The dynamic safe Euclidean distance between the end of the first or second front operating arm and the obstacle; For the current moment The column vector of residual external perturbation torques output by the generalized momentum observer.

[0013] Furthermore, the tension negative feedback correction mode follows this approach: ; In the formula, This is the command word for the reverse unloading torque. This is the preset tension negative feedback correction ratio; For a moment The actual detected contact torque value applied to the target object; The target contact force threshold is preset for the current operation stage.

[0014] A second aspect of the present invention provides a three-arm robot control system applicable to various types of switchgear operations. The control method described above is applied to a three-arm robot system, which includes a mobile platform, a lifting mechanism mounted on the front half of the mobile platform, a first front operating arm and a second front operating arm mounted on the lifting mechanism, and a rear monitoring arm mounted on the rear half of the mobile platform and equipped with a depth vision perception component. The first front operating arm and the second front operating arm are configured with heterogeneous functions, each equipped with a dexterous end effector and a dedicated working end effector, respectively. The control system includes: Environmental perception and mesh construction module: After driving the mobile platform to move to the preset work station, it drives the rear monitoring arm to perform multi-view scanning of the switch cabinet work area, constructs a priori static three-dimensional mesh of the work environment, and confirms the initial standby pose of the front first operating arm and the front second operating arm. Task parsing and decoupling module: used to match heterogeneous manipulators according to task attributes, calculate the nominal driving torque covering gravity and inertia compensation and send it down in real time, guide the dexterous end effector assembled on the first manipulator or the dedicated working end effector assembled on the second manipulator to perform normal contact actions, and ensure that the work chain starts stably under monitoring. Dynamic compensation and ranging module: used to acquire the real-time lifting displacement of the lifting mechanism during normal contact operation, and compensate for the relative coordinate offset between the rear monitoring arm and the front operating arm base caused by the lifting motion through homogeneous transformation matrix, and then calculate the dynamic safe Euclidean distance between the end of the front first operating arm or the end of the front second operating arm and the nearest obstacle in the switch cabinet environment. The dual-layer collision monitoring and response module is used to drive the built-in generalized momentum observer in parallel to eliminate interference from the lifting mechanism in order to extract the residual of the pure physical external disturbance torque; and modulates the residual by fusing dynamic safety Euclidean distance; when the cross-modal collision risk index exceeds the limit, the nominal normal contact operation trajectory of the front operating arm is interrupted, and asymmetric control is adaptively executed according to the heterogeneous attributes of the operating arm end that triggers the risk, so as to realize cross-modal monitoring and closed-loop control.

[0015] A third aspect of the present invention provides a computer-readable storage medium comprising a stored computer program, wherein the computer program, when executed by a processor, controls the device containing the storage medium to perform the operation method described above.

[0016] In summary, compared with the prior art, the above-described technical solutions conceived by this invention can achieve the following beneficial effects: 1. The control method of the present invention systematically solves the three core problems of task resource conflict, field of view obstruction and insufficient end-user adaptability in the prior art by three-arm division of labor, heterogeneous end-user matching, lifting compensation and cross-modal risk modulation, which significantly improves the stability, safety and intelligence level of switch cabinet multi-type operation.

[0017] 2. The control method of the present invention establishes a collision-free initial standby posture by achieving height alignment of the front first operating arm and the front second operating arm in virtual space and physical space, providing precise spatial position constraints and safe operation benchmarks for subsequent agile switching of multiple types of tasks, accurate obstacle avoidance, and field-of-view decoupling control.

[0018] 3. The control method of the present invention, under the safety constraints of a priori static three-dimensional mesh, deeply couples spatial trajectory planning with a dynamic model that integrates the height parameters of the lifting mechanism, and pre-calculates and issues nominal driving torque covering gravity compensation, Coriolis force and centrifugal force compensation, actively absorbing the physical impact caused by changes in the suspended base and mass distribution from the underlying servo drive level, ensuring the smoothness of motion and extremely high trajectory fidelity of the dexterous operation end and the dedicated operation end under complex contact conditions.

[0019] 4. The control method of the present invention completely eliminates the system-level ranging error introduced by the motion of the non-rigid time-varying base from the underlying logic of geometric coordinate transformation, ensuring that the digital twin engine can continuously output a dynamic safety Euclidean distance with extremely high fidelity within the full height coverage of various types of switch cabinet operations, thereby providing accurate and reliable cross-modal spatial distance prior constraints for the sensitivity modulation of the underlying generalized momentum observer.

[0020] 5. The control method of the present invention generates an exponentially amplified high-sensitivity response to minute physical contact when approaching the switch cabinet panel, thus winning an extreme time window for subsequent asymmetric compliant protection for different end tools, thereby thoroughly taking into account the continuity of cross-modal monitoring, the self-consistency of mathematical logic, and the ultimate operational safety. Attached Figure Description

[0021] Figure 1 This is a schematic diagram of the steps of the control method according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the structure of the three-arm robot system according to an embodiment of the present invention; Figure 3 This is a schematic diagram of the planar arrangement of the three manipulator bases on the mobile platform according to an embodiment of the present invention; Figure 4 This is a schematic diagram of the working space of the three operating arms relative to the moving platform body in an embodiment of the present invention; Figure 5 This is a planar distribution diagram of the workspace of the three operating arms relative to the moving platform body in an embodiment of the present invention; Figure 6 This is a schematic diagram of the control system according to an embodiment of the present invention. Detailed Implementation

[0022] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0023] Example 1 Please refer to Figures 1 to 5 This invention provides a three-arm robot control method applicable to various types of switchgear operations. The method is applied to a three-arm robot system, which includes a mobile platform, a lifting mechanism installed in the front half of the mobile platform, a first front operating arm and a second front operating arm installed on the lifting mechanism, and a rear monitoring arm installed in the rear half of the mobile platform and equipped with a depth vision perception component. The first front operating arm and the second front operating arm are configured with heterogeneous functions, each equipped with a dexterous end effector and a dedicated working end effector, respectively. The three-armed robot control method includes the following steps: S1: After the driving mobile platform moves to the preset work station, the driving rear monitoring arm performs multi-view scanning of the switch cabinet work area, constructs a priori static three-dimensional mesh of the work environment, and confirms the initial standby pose of the front first operating arm and the front second operating arm. S2: Match heterogeneous manipulators according to task attributes, calculate the nominal driving torque covering gravity and inertia compensation and send it out in real time, guide the dexterous end effector of the first manipulator or the dedicated working end effector of the second manipulator to perform normal contact actions, and ensure that the work chain starts stably under monitoring. S3: During normal contact operation, the real-time lifting displacement of the lifting mechanism is acquired in real time, and the relative coordinate offset between the rear monitoring arm and the front operating arm base caused by the lifting motion is compensated by the homogeneous transformation matrix. Then, the dynamic safe Euclidean distance between the end of the front first operating arm or the end of the front second operating arm and the nearest obstacle in the switch cabinet environment is calculated. S4: The parallel drive's built-in generalized momentum observer eliminates interference from the lifting mechanism to extract the residual of the pure physical external disturbance torque; and modulates the residual with a cross-modal risk index by fusing dynamic safety Euclidean distance; when the cross-modal collision risk index exceeds the limit, the nominal normal contact operation trajectory of the front operating arm is interrupted, and asymmetric control is adaptively executed according to the heterogeneous attributes of the operating arm end that trigger the risk, so as to realize cross-modal monitoring and closed-loop control.

[0024] In an optional embodiment, the first front operating arm and the second front operating arm are positioned on the left and right sides of the front half of the mobile platform, respectively, and are staggered in a non-collinear arrangement. It is understood that during use, by differentiating the staggered arrangement in terms of installation height, installation angle, or bracket extension, motion interference between the shoulder, elbow, and end effector can be reduced without significantly increasing the platform width, thereby increasing the usable workspace when both forearms are deployed simultaneously.

[0025] It should be noted that in this embodiment, the mobile platform is a common load-bearing mobile platform in the prior art, such as a quadruped or hexapod mobile platform. In other embodiments, other types of mobile platforms may be used, as long as they meet the requirements of robotic arm assembly, mobility and stability. No specific limitation is made here.

[0026] It should be noted that, in this embodiment, the dexterous operating end and the dedicated working end are common end effectors in the prior art, such as dexterous hands, compliant grippers, or grasping ends with high degrees of freedom, as well as infrared temperature measurement modules, rigid interface tools, specific screwing components, or other scenario-specific actuators. In other embodiments, other types of mobile platforms may also be used, as long as they meet the requirements of various types of operations in the switchgear. No specific limitations are made here.

[0027] It should be noted that, in this embodiment, the depth vision perception component mounted on the rear monitoring arm is a common vision inspection device in the prior art, such as one or more of a depth camera, RGB-D camera, structured light module, binocular vision module, infrared imaging module or lighting component. In other embodiments, other types of vision inspection devices may also be used, as long as they meet the requirements of various types of switch cabinet operations. No specific limitation is made here.

[0028] Specifically, in complex operation scenarios involving power switchgear, the distribution of operational targets is highly discrete and the working space is extremely narrow. Traditional robot systems often lack the ability to reconstruct the unstructured environment with high precision before physical execution, leading to a risk of collision for the front-end manipulator when entering the work area. Simultaneously, due to the pose uncertainty of the mobile platform and the lack of a unified coordinate reference between the robotic arm and the environment, it is difficult to establish a reliable initial operational benchmark within the complex cabinet, resulting in accumulated errors and field-of-view occlusion conflicts in subsequent refined operations. Therefore, this embodiment utilizes the independent motion sensing capability of the rear monitoring arm in step S1 to pre-construct a high-precision prior static 3D mesh on the switchgear surface before the actuator intervenes, thereby providing a globally blind-spot-free digital twin foundation for the entire system. Wherein: Step S1 includes: Step S11: Drive the mobile platform to achieve autonomous navigation using vehicle-mounted lidar and odometer; after arriving at the preset work station, level the chassis through ground support stiffness feedback to ensure that the bases of the first front operating arm and the second front operating arm are on the horizontal reference plane. Step S12: By using the depth sensing component carried by the rear monitoring arm to avoid the physical interference path of the front operating arm and perform surround visual sampling, a priori static three-dimensional mesh of the switch cabinet surface is constructed by projecting multiple frames of depth images onto the coordinate system of the mobile platform base. In an optional embodiment, the spatial position of the target object in the coordinate system of the mobile platform base is calculated as follows: ; In the formula, The three-dimensional coordinates of the target object in the coordinate system of the mobile platform base; The fixed homogeneous transformation matrix is ​​given for the rear monitoring arm base relative to the coordinate system of the mobile platform body. This is the time-varying transformation matrix of the camera coordinate system at the end of the rear monitoring arm relative to its base coordinate system; These are the original depth coordinates of the target object in the camera coordinate system.

[0029] Step S13: Load the constructed prior static 3D mesh into the digital twin engine, calculate the collision-free standby path of the first front manipulator and the second front manipulator in the current environment, drive the two manipulators to the initial working pose through joint angle closed-loop control, and use visual feedback to perform online alignment correction between the physical pose and the virtual model.

[0030] In an optional embodiment, the error calculation method for online alignment correction is as follows: ; In the formula, This refers to the online alignment deviation between the physical pose of the manipulator end effector and the virtual digital twin model. The actual physical space coordinates of the end effector of the manipulator in the coordinate system of the mobile platform base; These are the theoretical spatial coordinates of the end effector of the manipulator within the digital twin virtual space.

[0031] It should be noted that in step S13 of this embodiment, the prior static 3D mesh model constructed in step S12 is first imported into the built-in digital twin engine to establish a virtual work space that maps one-to-one with the physical space. The digital twin engine obtains the real-time joint angles of the first and second front manipulators and uses a forward kinematics model to reconstruct the current configuration of the two manipulators in the virtual space. Subsequently, by calling a path planning algorithm, such as the Fast Random Tree Search (RRT) algorithm, using the prior static 3D mesh as obstacle constraints and a preset operation starting point as the target point, a collision-free standby path is calculated for the two front manipulators from the current standby pose to the work start pose. During this path planning process, the dynamic envelope of the mobile platform body, the lifting mechanism, and the other manipulator must be considered simultaneously to ensure spatial safety during the coordinated movement of the two arms. During the path execution process, the calculated path point sequence is sent to each joint servo driver through the joint angle closed-loop control logic. The servo driver performs closed-loop adjustment of the position loop and velocity loop based on the real-time angle feedback from the joint encoder to ensure that the end effector of the manipulator moves strictly along the planned path. Once the manipulator reaches the preset initial working pose, the control system uses real-time point cloud data acquired by the rear monitoring arm to extract the physical feature points at the end of the manipulator and align them with the virtual feature points in the digital twin model. The system's mapping accuracy is confirmed by calculating the Euclidean distance deviation between the physical pose and the virtual model. If the alignment deviation is less than a preset safety threshold, the system is considered to have passed the check, and the robot proceeds to the next normal operation phase. If the deviation exceeds the threshold, an automatic calibration procedure or alarm will be triggered.

[0032] Specifically, in the complex operational scenarios of power switchgear, when facing tasks involving objects with significantly different types, such as buttons, knobs, cabinet door handles, and temperature measuring points, a single end effector or a single robotic arm often struggles to simultaneously achieve dexterity, rigidity, and coverage. Furthermore, when the front manipulator changes its vertical height under the drive of the lifting mechanism to adapt to different targets, the gravity gradient and mass inertia distribution of the entire system undergo dynamic perturbations. If the control system relies solely on simple kinematic trajectory planning without introducing global dynamic feedforward compensation for this variable base, the robotic arm will experience trajectory oscillations and driving torque instability at the moment of approaching and contacting the target, failing to guarantee reliable start-up and execution of various types of routine contact operations. Therefore, step S2 in this embodiment achieves precise decoupling between the operational target and the front manipulator through task attribute analysis. Through heterogeneous division of labor between the two forearms, frequent end-effector changes by a single robotic arm can be avoided, shortening the operational chain and improving the efficiency of switching between multiple task types. Wherein: Step S2 includes: Step S21: Divide the task to be executed into fine interaction tasks or positioning and guidance tasks; if the task involves pressing a button or turning a knob, activate the first front operating arm equipped with a dexterous end effector; if the task involves infrared temperature measurement or standard interface docking, activate the second front operating arm equipped with a dedicated operating end effector. Understandably, during operation, upon receiving a switchgear task instruction to be executed, the system will analyze the instruction in real time based on a preset task feature library. This analysis will identify the type of object to be operated, the required contact stiffness, and the dexterity requirements, thus classifying the task into either a fine-interaction task or a positioning-guided task. If the task is identified as a fine-interaction task, its core characteristic is that the end effector needs multi-degree-of-freedom attitude fine-tuning capabilities and compliant physical contact characteristics. In this case, the first operating arm equipped with a dexterous end effector will be activated, utilizing its dexterous hand or compliant gripper to complete the operation involving fine contact and attitude compensation. If the task is identified as a positioning-guided task, its core characteristic is that the end effector needs high structural stiffness, high load output capacity, or high tool specificity. In this case, the control system will activate the second operating arm equipped with a dedicated operating end effector, utilizing its infrared temperature measurement module, rigid interface tool, or specific turning assembly to perform operations requiring high stability or output torque.

[0033] Step S22: Within the established prior static 3D mesh, generate the desired working path of the end effector based on the target object position, and use the inverse kinematics algorithm to convert the desired working path into a sequence of target angles, target angular velocities, and target accelerations for each joint motor. Step S23: In real time, the current height of the lifting mechanism and the position of the manipulator are integrated, and the total torque required to maintain the stable movement of the robotic arm is calculated through positive dynamic compensation and sent to the servo drives of each joint.

[0034] In an optional embodiment, the nominal driving torque is calculated as follows: ; In the formula, This is the nominal driving torque vector sent to the motors of each joint of the manipulator; Here is the mass inertia matrix of the manipulator; Let be the desired acceleration vector in the joint space; The matrix of Coriolis force and centrifugal force of the manipulator; The desired angular velocity vector in joint space; This is the gravity compensation torque vector related to the lifting height.

[0035] Specifically, in the complex operating conditions of power switchgear, the bases of the first and second front manipulators move continuously vertically with the lifting mechanism, while the rear monitoring arm, which undertakes the task of global spatial perception, is fixed to the mobile platform body. This results in a continuous time-varying spatial displacement difference between the bases of these two types of heterogeneous robotic arms. Existing conventional robot control defaults to maintaining a fixed static coordinate mapping between the bases of multiple robotic arms. When facing such heterogeneous systems equipped with active lifting mechanisms, the fixed homogeneous coordinate transformation matrix will cause a severe tearing phenomenon between the virtual digital twin model and the real physical space coordinate system. This makes it impossible for the control system to accurately calculate the true minimum spatial Euclidean distance between the end effector of the first or second front manipulator and obstacles in the switchgear environment. Consequently, the subsequent safety collision protection mechanism will frequently generate false alarms or completely fail due to the distortion of the ranging data. Therefore, this embodiment incorporates the time-varying spatial attributes of the lifting mechanism into the cross-modal fusion link of visual perception and kinematic calculation through step S3. By collecting the vertical displacement data of the lifting mechanism in real time and constructing a dynamic translation transformation matrix, the spatial relative homogeneous mapping relationship between the rear monitoring arm and the front first and second operating arms is corrected at high frequency within the control system. Wherein: Step S3 includes: Step S31: The real-time lifting displacement data of the lifting mechanism relative to the mobile platform body in the vertical direction is obtained in real time by the displacement sensor installed at the drive end of the lifting mechanism. Step S32: Construct a time-varying translation transformation matrix based on real-time lifting displacement data, and combine it with the geometric installation relationship between the rear monitoring arm and the front first operating arm or the front second operating arm base to correct the relative spatial mapping relationship between the two in real time. In an optional embodiment, the compensation logic of the homogeneous transformation matrix follows the following coordinate transformation relationship: ; In the formula, For a moment The time-varying transformation matrix of the coordinate system of the first or second front operating arm relative to the coordinate system of the rear monitoring arm; The fixed homogeneous transformation matrix is ​​given for the rear monitoring arm base relative to the coordinate system of the mobile platform body. The time-varying translation transformation matrix is ​​determined by the real-time height of the lifting mechanism; It is a fixed homogeneous transformation matrix of the base of the first or second front operating arm relative to the mounting plane at the top of the lifting mechanism.

[0036] Step S33: Substitute the compensated spatial pose into the digital twin engine, and obtain the dynamic safe Euclidean distance by calculating the real-time coordinates of the end effector of the first or second front manipulator in the virtual space and the geometric relationship between the obstacle vertices in the prior static three-dimensional mesh.

[0037] In an optional embodiment, the dynamic safe Euclidean distance is calculated as follows: ; In the formula, For a moment The dynamic safe Euclidean distance; The real-time three-dimensional spatial coordinates of the end effector of the first or second front manipulator in the coordinate system of the mobile platform base; The set of obstacle vertices in the prior static 3D mesh constructed by the rear monitoring arm.

[0038] Specifically, in complex multi-type operations of switchgear, traditional robot collision detection faces a dual dilemma: limitations in perception modes and conflicts in underlying physical logic. On the one hand, pure visual monitoring inevitably suffers from physical occlusion and blind spots in contact force perception. Furthermore, when a momentum observer based solely on force perception is equipped with an active lifting mechanism, it is highly susceptible to misinterpreting non-rigid lifting of the base, sudden changes in gravity gradients, and structural flutter as external physical collisions, leading to frequent false stops. On the other hand, existing cross-modal fusion schemes, by directly and forcibly embedding time-varying visual spatial distance into the underlying momentum differential-integral stage, severely disrupt the physical causality of system dynamics. Moreover, the delay in the conventional software polling cycle prevents the system from achieving rapid and safe cutoff when approaching the target or experiencing minor contact, ultimately easily leading to control logic collapse or irreversible rigid physical damage to fragile switchgear components. Therefore, this embodiment constructs a two-layer cross-modal collision detection and rapid response architecture that decouples pure physical observation and visual modulation, conforming to rigorous physical causality, through step S4. Wherein: Step S4 includes: Step S41: Drive the built-in generalized momentum observer, inject the gravity compensation term and flutter energy absorption term related to the lifting mechanism in real time into the dynamic integration stage, and extract the pure physical external disturbance torque residual after removing the time-varying base interference by solving the integral deviation between the nominal momentum and the measured momentum over the historical time interval. In an optional embodiment, the solution method for the residual of the purely physical external disturbance torque is as follows: ; In the formula, , Each represents the current time. With historical moments The column vector of residual external perturbation torques extracted by the generalized momentum observer; This represents the current maximum actual runtime. For historical time dummy scalar variables within the integral operator; The observation gain matrix is ​​a diagonal constant. , At the current moment, either the first or second front manipulator is in position. The actual generalized momentum vector is zero at the initial moment; To be at a historical moment The nominal driving torque column vector is sent to each joint motor of the corresponding manipulator arm; To be at a historical moment The transpose of the Coriolis force and centrifugal force matrix corresponding to the manipulator; To be at a historical moment The time-varying gravity compensation torque vector; This is the mapping vector from the vibration velocity of the lifting mechanism to the joint disturbance torque; To be at a historical moment Scalar vibration velocity of the lifting mechanism actuator.

[0039] Step S42: Keeping the physical force control characteristics of the generalized momentum observer unchanged, the residual of the pure physical external disturbance torque is fused using the dynamic safety Euclidean distance to construct a cross-modal collision risk index; In an optional embodiment, the modulation method of the cross-modal collision risk index is as follows: ; In the formula, For a moment The collision risk index vector after cross-modal fusion; It is a unit diagonal matrix with the same dimension as the joint degrees of freedom of the first or second front manipulator. This is a visual sensitization gain diagonal matrix used to nonlinearly map spatial risk to the torque dimension of each joint; The space sensitivity decay constant; For a moment The dynamic safe Euclidean distance between the end of the first or second front operating arm and the obstacle; For the current moment The column vector of residual external perturbation torques output by the generalized momentum observer.

[0040] It should be noted that, in this embodiment, considering the computational boundaries of the digital control system, to prevent... Extremely small values ​​can cause floating-point overflow, affecting the calculated cross-modal collision risk index. A hard upper limit constraint is set; when the exponent term exceeds the preset maximum data type boundary, it is forcibly truncated to the preset safe maximum value to ensure the stability of the underlying servo communication.

[0041] Step S43: Perform a high-frequency comparison between the absolute values ​​of each joint component of the cross-modal collision risk index and the preset absolute safety threshold column vector; when any component in the cross-modal collision risk index exceeds the corresponding threshold in the absolute safety threshold column vector, forcibly freeze the nominal normal contact operation trajectory issued to the front first manipulator or the front second manipulator, and at the same time, extract the collision event data packet to determine the type of end effector mounted on the manipulator that triggered the risk; It should be noted that in this embodiment, in order to adapt to the complex working conditions of various types of switchgear operations, a single rigid collision threshold is not adopted; instead, based on the specific normal operation task currently being performed, such as a high-sensitivity micro-button pressing task or a high-load grounding switch crank rotation task, and the type of operating arm end effector activated by the current operation, such as a dexterous operating end effector or a dedicated operating end effector, the matching absolute safety threshold column vector is dynamically retrieved and called from the pre-loaded process safety recipe database.

[0042] Step S44: Execute an asymmetric response mechanism based on the extracted end effector type. If the risk is triggered by an operating arm equipped with a dedicated end effector, switch to torque control and activate the tension negative feedback correction mode. If the risk is triggered by an operating arm equipped with a dexterous operating end effector, immediately execute a zero stiffness resistance release action. Set the target driving torque of each joint servo driver of the operating arm to the sum of the real-time gravity compensation torque and the internal friction compensation of the system, so that the operating arm presents a passive retraction state of spring unloading in the force direction.

[0043] In an optional embodiment, the tension negative feedback correction mode follows the following manner: ; In the formula, This is the command word for the reverse unloading torque. This is the preset tension negative feedback correction ratio; For a moment The actual detected contact torque value applied to the target object; The target contact force threshold is preset for the current operation stage.

[0044] It should be noted that in switchgear operation, dedicated end effectors are typically used to perform tasks such as rotating grounding switch handles, connecting heavy-duty standard interfaces, or pushing and pulling circuit breaker trolleys. The core characteristic of these tasks is the need for a continuous and stable physical connection between the operating arm and the switchgear operating mechanism, and the output of significant force. When the operating arm equipped with this dedicated end effector triggers a cross-modal collision risk—that is, when the contact force or comprehensive risk index exceeds the safety threshold—conventional strategies such as directly unloading stiffness (softening the robotic arm) or immediately stopping and retracting will cause the tool to instantly detach from the switchgear operating port or handle. This not only leads to forced task failure but also easily causes the tool to fall and damage equipment, or causes severe secondary physical jamming of the internal mechanical transmission mechanism of the switchgear due to sudden changes in force posture. Therefore, it is necessary to proactively abandon the rigid position loop control strategy based on position error elimination; that is, instead of forcibly commanding the dedicated end effector to advance towards the original geometrical path point, the underlying control objective is transformed into maintaining the set torque boundary. In this mode, the reverse unloading torque command word is differentially canceled with the nominal drive torque in the underlying control bus, so that the manipulator exhibits mechanical compliance on a macroscopic scale, allowing the end effector to deviate from its original trajectory when encountering excessive resistance, thereby avoiding the destructive energy release caused by rigid compression.

[0045] Example 2 Please refer to Figure 6 This invention provides a three-arm robot control system applicable to various types of switchgear operations. The system comprises a mobile platform, a lifting mechanism mounted on the front half of the mobile platform, a first front operating arm and a second front operating arm mounted on the lifting mechanism, and a rear monitoring arm mounted on the rear half of the mobile platform and equipped with a depth vision perception component. The first front operating arm and the second front operating arm are configured with heterogeneous functions, each equipped with a dexterous end effector and a dedicated working end effector, respectively. The three-armed robot control system includes: Environmental perception and mesh construction module: After driving the mobile platform to move to the preset work station, it drives the rear monitoring arm to perform multi-view scanning of the switch cabinet work area, constructs a priori static three-dimensional mesh of the work environment, and confirms the initial standby pose of the front first operating arm and the front second operating arm. Task parsing and decoupling module: used to match heterogeneous manipulators according to task attributes, calculate the nominal driving torque covering gravity and inertia compensation and send it down in real time, guide the dexterous end effector assembled on the first manipulator or the dedicated working end effector assembled on the second manipulator to perform normal contact actions, and ensure that the work chain starts stably under monitoring. Dynamic compensation and ranging module: used to acquire the real-time lifting displacement of the lifting mechanism during normal contact operation, and compensate for the relative coordinate offset between the rear monitoring arm and the front operating arm base caused by the lifting motion through homogeneous transformation matrix, and then calculate the dynamic safe Euclidean distance between the end of the front first operating arm or the end of the front second operating arm and the nearest obstacle in the switch cabinet environment. The dual-layer collision monitoring and response module is used to drive the built-in generalized momentum observer in parallel to eliminate interference from the lifting mechanism in order to extract the residual of the pure physical external disturbance torque; and modulates the residual by fusing dynamic safety Euclidean distance; when the cross-modal collision risk index exceeds the limit, the nominal normal contact operation trajectory of the front operating arm is interrupted, and asymmetric control is adaptively executed according to the heterogeneous attributes of the operating arm end that triggers the risk, so as to realize cross-modal monitoring and closed-loop control.

[0046] Other technical features are the same as in Embodiment 1 and can achieve the same technical effects, so they will not be described in detail here.

[0047] It should be noted that the three-armed robot control system provided in this embodiment can be a computer program (including program code) running on a computer device. For example, the three-armed robot control system is an application program that can be used to execute the corresponding steps in the methods provided in the embodiments of the present invention.

[0048] In some feasible implementations, the three-armed robot control system provided in this embodiment can be implemented using a combination of hardware and software. As an example, the three-armed robot control system of this embodiment can be a processor in the form of a hardware decoding processor, which is programmed to execute the three-armed robot control method provided in this embodiment. For example, the processor in the form of a hardware decoding processor can be one or more application-specific integrated circuits (ASICs), digital signal processors (DSPs), programmable logic devices (PLDs), complex programmable logic devices (CPLDs), field-programmable gate arrays (FPGAs), or other electronic components.

[0049] In some feasible implementations, the three-armed robot control system provided in this embodiment can be implemented in software, which can be software in the form of programs and plug-ins, and includes a series of modules to implement the three-armed robot control method provided in this embodiment of the invention.

[0050] Example 3 This invention also provides a computer-readable storage medium storing a computer program that is executed by a processor to implement the various steps in the control method described above. For details, please refer to the implementation methods provided for the various steps described above, which will not be repeated here.

[0051] It should be understood that although the steps in the flowcharts of the accompanying figures are shown sequentially as indicated by the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in the flowcharts of the accompanying figures may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the sub-steps or stages of other steps.

[0052] It should be noted that if the embodiments of the present invention involve directional indicators (such as up, down, left, right, front, back, etc.), the directional indicators are only used to explain the relative positional relationship and movement of the components in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indicators will also change accordingly.

[0053] Furthermore, if the embodiments of this invention involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of the various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by this invention.

[0054] In this invention, the terms "comprising," "including," or any other variations thereof are intended to cover a 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 a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising..." does not exclude the presence of additional identical elements in the process, method, article, or apparatus that includes said element.

[0055] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit it; those skilled in the art will readily understand that the above descriptions are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for controlling a three-armed robot applicable to various types of operations in switchgear, characterized in that, This invention relates to a three-arm robot system, which includes a mobile platform, a lifting mechanism mounted on the front half of the mobile platform, a first front manipulator arm and a second front manipulator arm mounted on the lifting mechanism, and a rear monitoring arm mounted on the rear half of the mobile platform and equipped with a depth vision perception component. The first front manipulator arm and the second front manipulator arm are configured with heterogeneous functions, and are respectively equipped with a dexterous end effector and a dedicated working end effector. The control method includes the following steps: S1: After the driving mobile platform moves to the preset work station, the driving rear monitoring arm performs multi-view scanning of the switch cabinet work area, constructs a priori static three-dimensional mesh of the work environment, and confirms the initial standby pose of the front first operating arm and the front second operating arm. S2: Match heterogeneous manipulators according to task attributes, calculate the nominal driving torque covering gravity and inertia compensation and send it out in real time, guide the dexterous end effector of the first manipulator or the dedicated working end effector of the second manipulator to perform normal contact actions, and ensure that the work chain starts stably under monitoring. S3: During normal contact operation, the real-time lifting displacement of the lifting mechanism is acquired in real time, and the relative coordinate offset between the rear monitoring arm and the front operating arm base caused by the lifting motion is compensated by the homogeneous transformation matrix. Then, the dynamic safe Euclidean distance between the end of the front first operating arm or the end of the front second operating arm and the nearest obstacle in the switch cabinet environment is calculated. S4: The parallel drive's built-in generalized momentum observer eliminates interference from the lifting mechanism to extract the residual of the pure physical external disturbance torque; and modulates the residual with a cross-modal risk index by fusing dynamic safety Euclidean distance; when the cross-modal collision risk index exceeds the limit, the nominal normal contact operation trajectory of the front operating arm is interrupted, and asymmetric control is adaptively executed according to the heterogeneous attributes of the operating arm end that trigger the risk, so as to realize cross-modal monitoring and closed-loop control.

2. The control method according to claim 1, characterized in that, Step S1 includes: Step S11: Drive the mobile platform to achieve autonomous navigation using vehicle-mounted lidar and odometer; after arriving at the preset work station, level the chassis through ground support stiffness feedback to ensure that the bases of the first front operating arm and the second front operating arm are on the horizontal reference plane. Step S12: By using the depth sensing component carried by the rear monitoring arm to avoid the physical interference path of the front operating arm and perform surround visual sampling, a priori static three-dimensional mesh of the switch cabinet surface is constructed by projecting multiple frames of depth images onto the coordinate system of the mobile platform base. Step S13: Load the constructed prior static 3D mesh into the digital twin engine, calculate the collision-free standby path of the first front manipulator and the second front manipulator in the current environment, drive the two manipulators to the initial working pose through joint angle closed-loop control, and use visual feedback to perform online alignment correction between the physical pose and the virtual model.

3. The control method according to claim 2, characterized in that, Step S2 includes: Step S21: Divide the task to be executed into fine interaction tasks or positioning and guidance tasks; if the task involves pressing a button or turning a knob, activate the first front operating arm equipped with a dexterous end effector; if the task involves infrared temperature measurement or standard interface docking, activate the second front operating arm equipped with a dedicated operating end effector. Step S22: Within the established prior static 3D mesh, generate the desired working path of the end effector based on the target object position, and use the inverse kinematics algorithm to convert the desired working path into a sequence of target angles, target angular velocities, and target accelerations for each joint motor. Step S23: In real time, the current height of the lifting mechanism and the position of the manipulator are integrated, and the total torque required to maintain the stable movement of the robotic arm is calculated through positive dynamic compensation and sent to the servo drives of each joint.

4. The control method according to claim 3, characterized in that, Step S3 includes: Step S31: The real-time lifting displacement data of the lifting mechanism relative to the mobile platform body in the vertical direction is obtained in real time by the displacement sensor installed at the drive end of the lifting mechanism. Step S32: Construct a time-varying translation transformation matrix based on real-time lifting displacement data, and combine it with the geometric installation relationship between the rear monitoring arm and the front first operating arm or the front second operating arm base to correct the relative spatial mapping relationship between the two in real time. Step S33: Substitute the compensated spatial pose into the digital twin engine, and obtain the dynamic safe Euclidean distance by calculating the geometric relationship between the real-time coordinates of the end effector of the first or second front manipulator in virtual space and the obstacle vertices in the prior static 3D mesh. The dynamic safe Euclidean distance is calculated as follows: ； In the formula, For a moment The dynamic safe Euclidean distance; The real-time three-dimensional spatial coordinates of the end effector of the first or second front manipulator in the coordinate system of the mobile platform base; The set of obstacle vertices in the prior static 3D mesh constructed by the rear monitoring arm.

5. The control method according to claim 4, characterized in that, Step S4 includes: Step S41: Drive the built-in generalized momentum observer, inject the gravity compensation term and flutter energy absorption term related to the lifting mechanism in real time into the dynamic integration stage, and extract the pure physical external disturbance torque residual after removing the time-varying base interference by solving the integral deviation between the nominal momentum and the measured momentum over the historical time interval. Step S42: Keeping the physical force control characteristics of the generalized momentum observer unchanged, the residual of the pure physical external disturbance torque is fused using the dynamic safety Euclidean distance to construct a cross-modal collision risk index; Step S43: Perform a high-frequency comparison between the absolute values ​​of each joint component of the cross-modal collision risk index and the preset absolute safety threshold column vector; when any component in the cross-modal collision risk index exceeds the corresponding threshold in the absolute safety threshold column vector, forcibly freeze the nominal normal contact operation trajectory issued to the front first manipulator or the front second manipulator, and at the same time, extract the collision event data packet to determine the type of end effector mounted on the manipulator that triggered the risk; Step S44: Execute an asymmetric response mechanism based on the extracted end effector type. If the risk is triggered by an operating arm equipped with a dedicated end effector, switch to torque control and activate the tension negative feedback correction mode. If the risk is triggered by an operating arm equipped with a dexterous operating end effector, immediately execute a zero stiffness resistance release action. Set the target driving torque of each joint servo driver of the operating arm to the sum of the real-time gravity compensation torque and the internal friction compensation of the system, so that the operating arm presents a passive retraction state of spring unloading in the force direction.

6. The control method according to claim 5, characterized in that, The solution method for the residual of the purely physical external disturbance torque is as follows: ； In the formula, , Each represents the current time. With historical moments The column vector of residual external perturbation torques extracted by the generalized momentum observer; This represents the current maximum actual runtime. For historical time dummy scalar variables within the integral operator; The observation gain matrix is ​​a diagonal constant. , At the current moment, either the first or second front manipulator is in position. The actual generalized momentum vector is zero at the initial moment; To be at a historical moment The nominal driving torque column vector is sent to each joint motor of the corresponding manipulator arm; To be at a historical moment The transpose of the Coriolis force and centrifugal force matrix corresponding to the manipulator; To be at a historical moment The time-varying gravity compensation torque vector; This is the physical mapping column vector from helical vibration to the joint torque space; To be at a historical moment Scalar vibration velocity of the lifting mechanism actuator.

7. The control method according to claim 5, characterized in that, The modulation method of the cross-modal collision risk index is as follows: ； In the formula, For a moment The collision risk index vector after cross-modal fusion; It is a unit diagonal matrix with the same dimension as the joint degrees of freedom of the first or second front manipulator. This is the diagonal matrix for visual sensitization gain; The space sensitivity decay constant; For a moment The dynamic safe Euclidean distance between the end of the first or second front operating arm and the obstacle; For the current moment The column vector of residual external perturbation torques output by the generalized momentum observer.

8. The control method according to claim 1, characterized in that, The tension negative feedback correction mode follows the following approach: ； In the formula, This is the command word for the reverse unloading torque. This is the preset tension negative feedback correction proportional coefficient; For a moment The actual detected contact torque value applied to the target object; The target contact force threshold is preset for the current operation stage.

9. A three-arm robot control system applicable to various types of operations in switchgear, using the control method as described in any one of claims 1-8, characterized in that, This invention relates to a three-arm robot system, which includes a mobile platform, a lifting mechanism mounted on the front half of the mobile platform, a first front manipulator arm and a second front manipulator arm mounted on the lifting mechanism, and a rear monitoring arm mounted on the rear half of the mobile platform and equipped with a depth vision perception component. The first front manipulator arm and the second front manipulator arm are configured with heterogeneous functions, and are respectively equipped with a dexterous end effector and a dedicated working end effector. The control system includes: Environmental perception and mesh construction module: After driving the mobile platform to move to the preset work station, it drives the rear monitoring arm to perform multi-view scanning of the switch cabinet work area, constructs a priori static three-dimensional mesh of the work environment, and confirms the initial standby pose of the front first operating arm and the front second operating arm. Task parsing and decoupling module: used to match heterogeneous manipulators according to task attributes, calculate the nominal driving torque covering gravity and inertia compensation and send it down in real time, guide the dexterous end effector assembled on the first manipulator or the dedicated working end effector assembled on the second manipulator to perform normal contact actions, and ensure that the work chain starts stably under monitoring. Dynamic compensation and ranging module: used to acquire the real-time lifting displacement of the lifting mechanism during normal contact operation, and compensate for the relative coordinate offset between the rear monitoring arm and the front operating arm base caused by the lifting motion through homogeneous transformation matrix, and then calculate the dynamic safe Euclidean distance between the end of the front first operating arm or the end of the front second operating arm and the nearest obstacle in the switch cabinet environment. The dual-layer collision monitoring and response module is used to drive the built-in generalized momentum observer in parallel to eliminate interference from the lifting mechanism in order to extract the residual of the pure physical external disturbance torque; and modulates the residual by fusing dynamic safety Euclidean distance; when the cross-modal collision risk index exceeds the limit, the nominal normal contact operation trajectory of the front operating arm is interrupted, and asymmetric control is adaptively executed according to the heterogeneous attributes of the operating arm end that triggers the risk, so as to realize cross-modal monitoring and closed-loop control.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored computer program that, when executed by a processor, controls the device containing the storage medium to perform the manipulation method as described in any one of claims 1-8.

Images