Bionic multi-arm robot and cooperative control method thereof

Abstract

The application relates to the technical field of robot control, and discloses a bionic multi-arm robot and a cooperative control method thereof, which comprises the following steps: constructing a dynamics model comprising a main body, a motion mechanism and a working mechanism; extracting motion mechanism data to construct a dynamic support polygon; combining a working instruction to deduce an expected disturbance torque generated by the main body; dividing the working mechanism into a working arm and a compensation arm; when it is determined that the compensation ability for the expected disturbance torque is insufficient, the compensation arm is controlled to be converted into an extended configuration; defining a time window according to the dynamic support polygon; and when the working arm generates disturbance within the time window, the compensation arm is controlled to output a reverse joint acceleration to generate a reaction torque to offset the dynamics deviation of the main body. The application uses a non-working arm to reshape a global inertia tensor and generate a feedforward compensation torque, thereby inhibiting dynamic load disturbance at a physical topology level and improving the anti-overturning capability of the robot in a moving and supporting state.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, specifically to a biomimetic multi-arm robot and its cooperative control method. Background Technology

[0002] Bionic multi-arm robots combine the mobility of a bottom motion mechanism with the operational capabilities of multiple top working mechanisms, making them suitable for applications such as inspection in complex terrain and material handling. When the robot performs spatial grasping or contact operations, the acceleration and deceleration of the robotic arm, as well as changes in external load, transmit inertial disturbance torques to the robot body. Because the bottom motion mechanism continuously switches its contact state with the ground during alternating steps, the robot's dynamic support polygon changes continuously over time, resulting in fluctuating anti-tipping capabilities.

[0003] Existing robot control strategies typically plan chassis motion and robotic arm operations independently. To prevent the robotic arm's movement from causing a shift in the center of gravity, conventional control methods often employ speed limiting, which involves forcibly reducing the joint acceleration of the robotic arm during operation, or requiring the robot to completely stop moving before performing the end-effector task. This reduces the operational efficiency and motion continuity of multi-arm systems.

[0004] Meanwhile, existing control methods primarily rely on adjusting the base to change the gait landing point or lowering the robot's height to passively maintain balance when dealing with operational disturbances. For robots equipped with multiple robotic arms, idle robotic arms not performing current tasks are usually locked in a fixed posture. Their own mass distribution and joint actuators fail to participate in the overall dynamic balance adjustment, resulting in a waste of system physical resources. When the robot performs highly dynamic operations or faces sudden changes in external loads, relying solely on base adjustments often cannot generate a sufficiently large compensating torque to offset the impact. This can easily cause the robot's actual zero-moment point to fall outside the current support polygon range, leading to robot instability or overturning. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a biomimetic multi-arm robot and its collaborative control method, which solves the problem that when existing biomimetic multi-arm robots perform tasks, the movement of the robotic arm transmits unknown inertial disturbance torque to the base, which can easily cause the robot to lose balance and tip over while moving or supported.

[0006] To achieve the above objectives, the present invention provides the following technical solution: A biomimetic multi-arm robot and its cooperative control method, comprising constructing a dynamic model including a main body, a motion mechanism and multiple working mechanisms; Extract the motion cycle data of the motion mechanism to construct a dynamic support polygon, and combine it with the operation instructions received by the working mechanism to deduce the expected disturbance torque generated by the operation instructions on the main body based on the dynamic model; According to the work instructions, the multiple working mechanisms are divided into working arms and compensation arms. When it is determined that the compensation capability for the expected disturbance torque is insufficient, an extension command is issued to the compensation arm to control the compensation arm to convert to an extended configuration. Based on the area of ​​the dynamic support polygon, a time window is defined. Within the time window and when the working arm's movement causes a disturbance, the compensating arm in the extended configuration outputs a reverse joint acceleration, generating a reaction torque to counteract the dynamic offset experienced by the main body.

[0007] This invention reshapes the global inertia tensor of a multi-arm system by actively changing the spatial configuration of the non-operating arm, and uses limb movements to generate feedforward canceling torques to suppress base instability caused by dynamic loads at the physical topology level.

[0008] Furthermore, the construction of the dynamic model, which includes the main body, the motion mechanism, and multiple working mechanisms, further includes the step of solving the spatial coordinates of the actual zero-moment point, specifically including: Obtain the pose state vector of the main body, the underlying drive vector of the motion mechanism, and the independent joint vector of the working mechanism, and concatenate the pose state vector, the underlying drive vector, and the independent joint vector to generate the system's global generalized coordinate vector; A Lagrangian function is established based on the physical parameters and real-time motion speed of each link. The corresponding parameter matrix is ​​extracted based on the Lagrangian function. A multi-rigid-body dynamic equation is generated as the dynamic model by combining the contact constraint mapping model applied by the ground to the motion mechanism. Based on the dynamic model, the spatial acceleration of the machine's center of mass and the rate of change of spatial angular momentum around the center of mass are calculated. The two-dimensional plane coordinates of the actual zero-moment point in the current state are deduced, and it is determined whether the two-dimensional plane coordinates fall within the range of the dynamic support polygon.

[0009] Furthermore, a protective pad is provided at the bottom of the motion mechanism, and the step of extracting the motion cycle data of the motion mechanism to construct a dynamic support polygon and define a time window includes: Based on the angular displacement and angular velocity parameters of the motion mechanism, the height trajectory of the protective pad is deduced, and a prediction sequence of the contact state of the protective pad changing over time is generated. The ground contact state parameters in the contact state prediction sequence are extracted and projected onto the horizontal plane as the ground position data. The geometric area of ​​the region enclosed by the two-dimensional coordinate point set is calculated using the convex hull algorithm, and a support area function that changes with time is generated. The supporting area function is compared with the area safety threshold. In the future prediction time domain, continuous time segments in which the area calculation result is greater than or equal to the area safety threshold are extracted, and a safe operation time window interval is generated as the time window.

[0010] Furthermore, the step of dividing the multiple working mechanisms into working arms and compensating arms according to the work instructions, and issuing an extension instruction to the compensating arms when it is determined that the compensation capability for the expected disturbance torque is insufficient, includes: Calculate the Euclidean distance between the end of each working mechanism and the target object, and solve the inverse kinematics solution to reach the target object by combining the kinematic model of each working mechanism to generate a set of reachable working mechanisms. In the set of reachable working mechanisms, the working mechanism closest to the target object is classified as the working arm, and the working mechanisms that are not working arms are classified as the compensation arm. Based on the dynamic model, the coupling parameter matrix is ​​extracted, and the equivalent disturbance spinor transmitted from the operation action to the center of mass of the main body is calculated as the expected disturbance torque. The maximum equivalent compensation torque of the compensation arm under the current configuration is calculated using the Jacobian matrix transpose mapping. The dynamic capability margin coefficient is generated by calculating the ratio of the modulus of the maximum equivalent compensation torque to the modulus of the expected disturbance torque. When the dynamic capability margin coefficient is less than the set margin safety threshold and the remaining time of the predicted takeoff time is greater than the time required for configuration switching, the compensation capability is determined to be insufficient, so as to quantify the dynamic reserve capability of the multi-arm robot to resist external impacts.

[0011] Furthermore, controlling the compensation arm to convert to an extended configuration includes: Extract the mass parameters and local rotational inertia of each link in the compensation arm, and calculate the global inertia tensor matrix of the compensation arm equivalent to the center of mass of the main body; The global inertia tensor matrix is ​​projected onto the force direction, and an inertial performance index function is constructed based on the rotational inertia of the compensating arm in the disturbance axis. The gradient optimization algorithm is used to calculate the partial derivative vector of the inertial performance index function with respect to the joint position variables. An iterative update operation with mechanical limit truncation is performed. The joint position variables corresponding to the gradient magnitude being less than the convergence threshold are used as the target repositioning and sent to the compensation arm for execution, so that the compensation arm is converted into the extended configuration. In this way, the equivalent rotational inertia in the disturbance direction is maximized through gradient optimization, thereby improving the physical impedance of the structure.

[0012] Furthermore, the step of controlling the compensation arm to convert to an extended configuration also includes a step of constraining the arm using a zero-space projection control law without interfering with the working arm, specifically including: Extract the kinematic Jacobian matrix corresponding to the main task, calculate the pseudo-inverse matrix of the kinematic Jacobian matrix using the damped least squares method, and construct the null space projection operator based on the pseudo-inverse matrix and the identity matrix. The main joint acceleration command is calculated by combining the target trajectory contained in the operation command with the current joint state feedback. The secondary joint acceleration command is calculated by combining the target reshaped pose with the current joint state feedback of the compensating arm. The main joint acceleration command and the secondary joint acceleration command are then extended into a global command vector. The secondary joint acceleration command is orthogonally mapped and filtered using the null projection operator. The mapped and filtered secondary joint acceleration command is superimposed on the primary joint acceleration command to synthesize a decoupled control law command vector. The decoupled control law command vector is input into the inverse dynamics model to calculate the driving torque and send it down for control. The projection operator is used to filter out the motion components caused to the end of the working arm, thereby realizing the kinematic decoupling between the compensation action and the working action.

[0013] Furthermore, when the working arm's movement causes a disturbance within the time window, the compensating arm in the extended configuration outputs a reverse joint acceleration, including: The time period during which the motion mechanism meets the preset number of supports and overlaps with the ground is defined as the gait phase-locked window. When the system is in the gait phase-locked window and within the time window, the feedforward compensation mechanism is activated. Obtain the time difference between the current absolute time and the start time of the gait cycle, calculate the quotient of the time difference and the predicted cycle time and truncate it, and map it to generate a normalized phase variable as a motion trajectory synchronization reference. The main body compensation torque is calculated by combining the global inertia tensor matrix and the disturbance rejection angular acceleration requirement. The main body compensation torque is mapped to the joint feedforward torque vector of the compensation arm by transposing the Jacobian matrix. The joint feedforward torque vector is superimposed on the feedback driving torque calculated by the inverse dynamics model to synthesize the global control torque and send it to the compensation arm.

[0014] Furthermore, after generating the reaction torque to counteract the dynamic offset experienced by the body, the process further includes the step of performing safety degradation control: Continuously monitor the actual zero-moment point spatial coordinates and the configuration state of the compensating arm; when it is determined that the actual zero-moment point spatial coordinates fall outside the range of the dynamic support polygon, or when it is determined that the compensating arm cannot complete the configuration reshaping before the predicted ground lift-off time, reduce the target joint acceleration of the working arm, suspend the end contact action, or adjust the gait phase of the motion mechanism. When the actual zero-moment point spatial coordinates re-enter the range of the dynamic support polygon and the dynamic capability margin coefficient is determined to have recovered to above the margin safety threshold, the normal collaborative control process is restored to prevent the system from overturning due to prediction errors or extreme operating conditions.

[0015] A second aspect of the present invention provides a biomimetic multi-arm robot applied to the cooperative control method described in the first aspect above, comprising a main body, a plurality of working mechanisms fixedly connected to the top of the main body, a motion mechanism fixedly connected to the bottom of the main body, and a controller provided on the main body, the controller being configured to execute the cooperative control method described above. A probe is installed at the front end of the main body; The working mechanism includes a mounting base, a mounting plate rotatably connected to the top of the mounting base, a connecting arm rotatably connected to the top of the mounting plate via a first rotating shaft, a second rotating shaft being provided at the top of the connecting arm and located at the output end of a motor, a working arm rotatably connected to the outside of the motor, and a multi-functional claw being installed at the bottom of the working arm.

[0016] Furthermore, the motion mechanism includes a main board, a frame is fixedly connected to the outside of the main board, a rotating shaft is rotatably connected to the outside of the frame, a long plate is mounted on the outside of the rotating shaft, a connecting rod is rotatably connected to the bottom of the long plate, and a protective pad is wrapped around the bottom of the connecting rod.

[0017] This invention provides a biomimetic multi-arm robot and its cooperative control method. It has the following beneficial effects: 1. This invention divides the working arm and the compensation arm according to the operation instructions, and controls the compensation arm to switch to an extended configuration when the expected compensation capacity is insufficient. Combined with the time window defined by the dynamic support polygon, the compensation arm outputs the reverse joint acceleration. The non-working arm changes the global inertia tensor of the whole machine and generates a feedforward canceling torque, thereby suppressing the disturbance of dynamic load on the base at the physical topology level and improving the anti-tipping ability of the robot when it is in a moving or supported state.

[0018] 2. This invention introduces a zero-space projection control law when controlling the extension of the compensating arm, and constructs a projection operator using the pseudo-inverse of the Jacobian matrix of the main task kinematics. It then performs orthogonal mapping filtering on the acceleration commands of the secondary joints, constraining the motion displacement of the compensating arm within the zero space of the main task. This ensures that the compensating arm will not interfere with the end-operation trajectory of the working arm when it changes the mass distribution of the system and exerts reverse force, thus achieving kinematic decoupling between the anti-disturbance compensation action and the high-priority task.

[0019] 3. This invention defines a safe operating time window by deriving the ground contact state sequence of the motion mechanism, activates a feedforward compensation mechanism within the gait phase-locked window, and simultaneously monitors the actual zero-moment point coordinates and dynamic capability margin coefficient. When the system is determined to be facing instability risk, it actively executes safety degradation control to reduce the target acceleration or adjust the gait phase, thereby avoiding system collapse under the conditions of prediction error or sudden interference, and ensuring the safety and mechanical stability of the multi-arm robot operation process. Attached Figure Description

[0020] Figure 1 This is a perspective view of the present invention; Figure 2 This is a schematic diagram of the working mechanism of the present invention; Figure 3 This is a schematic diagram of the motion mechanism of the present invention; Figure 4 This is a flowchart of the method of the present invention; Figure 5 This is a flowchart of the dynamic support polygon and safe operation time window extraction process of the present invention; Figure 6 This is a flowchart of the multi-arm role classification and feedforward quantization process of the present invention; Figure 7 This is a flowchart of the compensation capability assessment and limit lever arm configuration triggering process of the present invention; Figure 8 This is a flowchart of the zero-space cascaded projection and decoupling control law construction of the present invention.

[0021] The components are as follows: 1. Main body; 2. Working mechanism; 21. Mounting base; 22. Mounting plate; 23. Connecting arm; 24. Rotating shaft; 25. Motor; 26. Working arm; 27. Multifunctional claw; 28. Second rotating shaft; 3. Motion mechanism; 31. Main board; 32. Frame; 33. Rotating shaft; 34. Long plate; 35. Connecting rod; 36. Protective pad; 4. Probe head. Detailed Implementation

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

[0023] See attached document Figure 1 - Appendix Figure 3This invention provides a biomimetic multi-armed robot, comprising a main body 1, with multiple working mechanisms 2 fixedly connected to the top of the main body 1, a motion mechanism 3 fixedly connected to the bottom of the main body 1, and a probe head 4 installed at the front end of the main body 1. The probe head 4 serves as an environmental information input terminal, providing spatial coordinate data of external target objects.

[0024] The working mechanism 2 includes a mounting base 21, with a mounting plate 22 rotatably connected to the top of the mounting base 21. A connecting arm 23 is rotatably connected to the top of the mounting plate 22 via a first rotating shaft 24, and a second rotating shaft 28 is mounted on the top of the connecting arm 23. The output end of the motor 25 is fixedly connected to the second rotating shaft 28, and a working arm 26 is rotatably connected to the outside of the motor 25. A multi-functional claw 27 is mounted on the bottom of the working arm 26. Each working mechanism 2 is equipped with an independent drive feedback loop, and each mechanical transmission node is equipped with an encoder to provide real-time joint space data to the controller.

[0025] The motion mechanism 3 includes a main board 31, to which a frame 32 is fixedly connected. A rotating shaft 33 is rotatably connected to the outer side of the frame 32, and a long plate 34 is mounted on the outer side of the rotating shaft 33. A connecting rod 35 is rotatably connected to the bottom of the long plate 34, and a protective pad 36 covers the bottom of the connecting rod 35. An inertial measurement module is deployed inside the main board 31 to acquire real-time spatial state data of the base. Joint state sensors are configured on the rotating shaft 33 and the connecting rod 35 to collect motion parameters.

[0026] See attached document Figure 4 This invention provides a cooperative control method for a biomimetic multi-armed robot, executed by a processing unit within a motherboard 31, comprising the following steps: S1, construct the global multi-rigid-body dynamics topology and solve the zero-moment point state, synchronously read the data of probe head 4, attitude sensor and encoder of each drive node, establish a unified generalized coordinate system including main body 1, motion mechanism 3 and all working mechanism 2, establish the whole machine dynamics equation based on real-time sensing parameters and solve the actual zero-moment point spatial coordinates of the robot under the current physical state; S2, extract chassis gait intention and operational disturbance feedforward evaluation, extract motion cycle data of rotating shaft 33 and connecting rod 35, construct dynamic support polygon geometric model that progresses over time, and combine the predetermined operational trajectory command received by the current working mechanism 2 to forward deduce the expected disturbance torque amplitude and direction generated by the task execution action on the main board 31. S3 triggers the pre-configuration and inertia tensor reshaping of the compensating arm. According to the work instructions, the multiple working mechanisms 2 are divided into working arms and compensating arms. When it is determined that the physical compensation capability is insufficient and the protective pad 36 is lifted off the ground, an extension command is sent to the motor 25 of the compensating arm to increase the rotation radius of each link. The compensating arm is constrained by the zero-space projection control law to complete the limit lever arm extension without interfering with the spatial posture of the working arm. S4 executes phase-locked coordination and dynamic compensation based on gait hardware cycle. It defines the time window according to the real-time area of ​​the dynamic support polygon and issues angular acceleration adjustment instructions to each working mechanism 2 according to the time window. When the working arm starts working and generates disturbance, the compensation arm in the extension configuration outputs the reverse joint acceleration according to the feedforward compensation instruction to generate a reaction torque to counteract the dynamic offset of the main body 1.

[0027] The collaborative control method provided by this invention establishes a unified spatial description model including the main body 1, the motion mechanism 3, and all working mechanisms 2 before executing the underlying control algorithm. Specifically, it includes the following steps for extracting system state parameters and mapping coordinate systems: S101, Establish the global reference benchmark and the floating base state space. A global absolute coordinate system is established using the ambient ground as a static reference benchmark, and a local connected coordinate system is established with the geometric center of the main board 31 as the origin. Due to the dynamic displacement and attitude changes generated by the alternating steps of the connecting rod 35, the main body 1 exhibits six degrees of freedom motion characteristics in space. The processing unit acquires attitude angular velocity and linear acceleration data through the inertial measurement module built into the main board 31, and combines this with the joint state data of the motion mechanism 3, the ground contact constraint information of the protective pad 36, and the environmental positioning information output by the probe 4 to perform fusion estimation, extracting the pose state vector of the main board 31 in the global coordinate system. The pose state vector contains three-dimensional translation data reflecting the spatial position of the motherboard 31 and three-dimensional rotation Euler angle data reflecting the spatial attitude. When using Euler angles to represent the attitude, the rotation of the Euler angles is set sequentially around the Z-axis, Y-axis, and X-axis. When the attitude angle approaches the Euler angle singularity region, the processing unit switches to quaternions or rotation matrices for attitude representation and calculation to avoid divergence in attitude calculation results and ensure the continuity of control calculation.

[0028] S102, extract the independent drive joint variables of motion mechanism 3. The drive source controls the angular displacement of rotation axis 33, and defines this angular displacement state as the bottom-level drive vector. The long plate 34 and the connecting rod 35 are connected to the rotation axis 33 via mechanical hinges to form a spatial mechanism, the position of which is determined by the angular displacement of the rotation axis 33. For the analysis of the follow-up position of the long plate 34 and the connecting rod 35 in the spatial mechanism, those skilled in the art can use the closed-loop vector polygon method to construct a system of position constraint equations for solution. The derivation of its forward kinematics is a well-known technique in the field and will not be elaborated here.

[0029] The system only incorporates the real-time angular displacement of the actively rotating axis 33 into the underlying drive vector. This reduces the computational complexity caused by the coupling of the degrees of freedom of the underlying mechanisms.

[0030] S103, extract the kinematic joint vectors of the top working mechanism 2. For the part fixed to the top of the main body 1... For the second working mechanism, the system synchronously collects joint data through encoders configured at each transmission node. (This is for the first...) The system acquires the first yaw angle of the mounting plate 22 rotating horizontally relative to the mounting base 21, the second pitch angle of the connecting arm 23 oscillating about its bottom first pivot 24, and the third pitch angle of the working arm 26 rotating relative to the top second pivot 28 driven by the motor 25. The processing unit transposes and integrates these three angle parameters according to the serial topology of the robotic arm to generate the first... Independent joint vectors of work mechanism 2 subscript The value range is 1 to The natural number.

[0031] S104, Construct the system's global generalized coordinate vector. The system processing unit will then process the pose state vector. Low-level driving vectors And the independent joint vectors of all working mechanisms 2 Perform matrix concatenation to generate a matrix with dimension 1. The system's global generalized coordinate vector This vector serves as the fundamental input parameter for subsequent multi-rigid-body dynamics solutions, and its structure is expressed as follows: ; in, The total number of working mechanisms attached to the robot, superscript. This represents the matrix transpose operation, dimension depending on , and each The total number of elements.

[0032] S105, establish the homogeneous transformation topology of the task execution end. To calculate the spatial coordinates of the end effector of the multi-functional claw 27 and perform collision detection, the system constructs a forward kinematic transmission chain based on the set link lengths and physical geometric parameters. The physical geometric parameters are pre-obtained using the standard DH kinematic parameter calibration method and stored in the memory of the motherboard 31. Based on the connected coordinate system of the motherboard 31, the pose transformation of each mechanical connection point is derived using a fourth-order homogeneous transformation matrix. The comprehensive transformation matrix from the motherboard 31 to the end effector of the multi-functional claw 27 of the designated working mechanism 2 is obtained by successive multiplication of the fixed position transformation matrix of the mounting base 21 relative to the motherboard 31 and the local rotation and translation homogeneous matrices generated by each rotating joint in the working mechanism based on its real-time angle parameters. The system constructs a digital topology mapping model of the entire robot accordingly.

[0033] The cooperative control method provided by this invention, after establishing the global generalized coordinate vector of the system, constructs equations reflecting the overall mass distribution and force state of the machine. The dynamic model establishes the mapping relationship between the motion state of each joint, the driving torque, and the contact forces, providing a physical basis for attitude control. Specifically, it includes the following steps for dynamic parameter extraction and equation construction: S106, Extract system mass attributes and construct a Lagrangian function. The system processing unit reads the preset physical parameters of each mechanical component from the memory of the motherboard 31. The physical parameters include the mass distribution values, center-of-mass position data, and rotational inertia tensors about the principal axes of the center-of-mass of each link in the main body 1, motion mechanism 3, and each working mechanism 2. Based on the physical parameters of each component and the real-time motion velocity parameters, the processing unit calculates the total kinetic energy and total potential energy of the robot in its current motion state. According to the principles of analytical mechanics, the system uses the difference between the total kinetic energy and the total potential energy to establish a Lagrangian function. For the partial derivative calculation of the Lagrangian function, those skilled in the art can write a calculation program based on the fundamental theorems of analytical mechanics. The energy derivation calculation process is a well-known technique in the field and will not be described in detail here.

[0034] S107, Analyze the system matrix equations under unconstrained conditions. Based on the obtained Lagrange partial derivatives, the processing unit separates the parameter terms related to joint acceleration and joint velocity, constructing the dynamic parameter matrix under non-contact conditions. The processing unit extracts the system's global generalized coordinate vector. Related generalized inertia matrix The dimension of this matrix depends on the total number of elements in the global generalized coordinate vector, reflecting the inertial coupling relationship of the multi-arm system under different configurations. The system extracts the Coriolis force and centrifugal force matrices characterizing the nonlinear coupling effect. ,in Generalized coordinate vector The first derivative vector with respect to time. The processing unit extracts the gravity vectors associated with each mechanical component. The system collects real-time torque data from the configured servo motors and drive sources, and integrates it to generate a global generalized drive torque vector. .

[0035] S108, Establish a contact constraint mapping model between the bottom mechanism and the ground. During gait cycles, the protective pad 36 at the bottom of the linkage 35 alternately contacts the ground, and the ground applies a reaction force to the supported parts. The processing unit acquires contact force feedback data applied by the ground to the protective pad 36 through a contact force sensor installed within the protective pad 36. When the contact force feedback value exceeds a set ground contact threshold, the corresponding protective pad 36 is determined to be in a supported state. The processing unit extracts the three-dimensional spatial coordinates of each protective pad 36 in a supported state and calculates the contact Jacobian matrix of the contact point in the current generalized coordinate system. The contact Jacobian matrix reflects the mapping relationship between the joint space velocity and the Cartesian space velocity at the contact point. The processing unit extracts the corresponding mechanical feedback values ​​based on this matrix and generates the contact reaction vector. .

[0036] S109, Construct a set of dynamic equations including force constraints. The processing unit combines the kinematic attribute matrix with the terrain constraint force parameters using algebraic equations to generate multi-rigid-body dynamic equations under contact-constrained conditions. The algebraic expressions of the dynamic equations are as follows: ; in, The system's global generalized coordinate vector The vector of the second derivative with respect to time, with superscript This represents the matrix transpose operation. The left side of the equation represents the resultant force of the system's inertial force, Coriolis force, and gravity generated by the robot in motion. The right side combines the active output torque of the robot's actuators and the reaction force from the terrain support applied externally. The processing unit uses this to build a dynamic physical model, supporting the derivation and calculation of the system's center of gravity.

[0037] The cooperative control method provided by this invention quantifies and extrapolates the stability indices of the robot after constructing a multi-rigid-body dynamic equation system. The zero-moment point, as a physical parameter measuring the anti-tipping capability of the mobile base, reflects the equivalent point of action of gravity and inertial forces on the contact surface. Specifically, it includes the following steps: centroid state calculation and zero-moment point coordinate derivation: S110, Calculate the spatial center of gravity position of the entire machine. The system processing unit collects the mass parameters and center of gravity position data of each link in the main body 1, motion mechanism 3, and all working mechanisms 2. The processing unit sums the products of the mass of each link and its corresponding spatial coordinate, and divides the sum by the total mass of the system. The three-dimensional orthogonal coordinate components of the machine's center of mass in the global coordinate system are calculated and denoted as the lateral components. Longitudinal component and vertical components .

[0038] S111, calculate the acceleration of the center of mass and the rate of change of angular momentum. Based on the second derivative of the generalized coordinate vector with respect to time calculated from the dynamic equations, and combined with the system's kinematic Jacobian matrix, the processing unit obtains the spatial acceleration of the entire machine's center of mass through forward kinematic differential calculation. The processing unit extracts the components of this spatial acceleration in three directions in the global coordinate system, denoted as follows: , as well as .

[0039] Simultaneously, the processing unit calculates the spatial angular momentum change rate vector of the entire machine about its center of mass based on the rotational inertia tensor and angular acceleration data of each link. The projection components of this angular momentum change rate vector onto the X and Y axes of the global coordinate system are extracted and denoted as follows: and For matrix operations involving solving the differentials of the system's center of mass acceleration and angular momentum using the Jacobian matrix, those skilled in the art can construct transformation operators based on fundamental robotics theories. The process of solving the kinematic differentials is well-known in the field and will not be elaborated upon here.

[0040] S112, Deducing the two-dimensional plane coordinates of the zero-moment point. The zero-moment point refers to the reference point where the sum of the horizontal overturning moments generated by the forces acting on the system's contact surface with the ground is zero. The processing unit constructs the algebraic relationship of the zero-moment point coordinates based on the center of mass position component, center of mass acceleration component, and rate of change of angular momentum component. Under the condition that the ground is considered a horizontal reference surface, the system uses the plane with zero Z-axis coordinate as a reference to deduce the horizontal axis coordinates of the overall zero-moment point in the current state. with the vertical axis coordinate .

[0041] When the ground has a slope or local undulations, the processing unit fits the current contact support plane based on the spatial coordinates of the protective pad 36 in the supported state, and projects the coordinates of the zero moment point and the contact point coordinates of the protective pad 36 into the contact support plane to determine the stability. The solution equation is as follows: ; ; in, This is the gravitational acceleration constant. The system calculates the two-dimensional coordinates of the zero-moment point of the entire machine based on this. The processing unit determines whether these zero-moment point coordinates fall within the range of the support polygon formed by the lines connecting the positions of the protective pads 36 in their supported state. This positional relationship is used as the physical basis for subsequent assessment of whether the main body 1 has overturned. The system thus completes the global physical state quantification within the current control cycle.

[0042] See attached document Figure 5 The collaborative control method provided by this invention analyzes the motion rhythm of the chassis hardware before executing the operation command to calculate the evolution trend of the robot base's anti-tipping capability over a future period. Because the robot's support state cyclically switches between multiple contact points during alternating gait, the support area exhibits periodic fluctuations over time. The system extrapolates a safe period with a wide support base to plan the upper robotic arm's operation, specifically including the following time-series extrapolation and time window extraction steps: S201, extract the underlying drive phase and cycle parameters. Motion mechanism 3 is controlled by a drive source; the rotating shaft 33 performs periodic rotation, driving the long plate 34 and connecting rod 35 to complete alternating stepping movements. The system processing unit reads the angular displacement and angular velocity of the rotating shaft 33 through an encoder. Based on the angular displacement and angular velocity parameters, the processing unit calculates the basic phase of the current gait cycle and estimates the time required for motion mechanism 3 to complete a full gait cycle.

[0043] S202, Construct a contact state prediction sequence. Since the dimensions of the motion mechanism 3 components are fixed, there is a rigid geometric mapping relationship between the height trajectory of the bottom protective pad 36 of the connecting rod 35 and the rotation phase of the rotating shaft 33. The system sets the future prediction time variable. Based on the current fundamental phase and angular velocity, the predicted phase of the rotating axis 33 at future moments is deduced. The processing unit then calculates the future phase of each protective pad 36 based on the predicted phase. The system establishes a height reference zero point based on the current horizontal ground level and sets a ground contact judgment tolerance due to mechanical clearance and deformation. When the predicted height is less than or equal to this ground contact judgment tolerance, the ground contact state parameter of the corresponding protective pad 36 is assigned the value of "grounded"; when the predicted height is greater than this ground contact judgment tolerance, the ground contact state parameter is assigned the value of "off the ground". The system then generates a time-varying contact state prediction sequence for each protective pad 36.

[0044] S203, Calculate the area of ​​a dynamically supported polygon. Predict future time variables. Within the defined prediction domain, the processing unit extracts the position data of the protective pad 36 with the ground contact state parameter at a preset time step. The processing unit projects the extracted position data onto a horizontal plane to obtain a set of two-dimensional coordinate points. The system uses a convex hull algorithm to calculate the geometric area of ​​the region enclosed by this set of two-dimensional coordinate points, generating a support area function that varies with time. For the convex hull algorithm that calculates the area of ​​a convex polygon using a set of two-dimensional coordinate points, those skilled in the art can write programs to implement it based on the principles of computational geometry. The process of finding points and calculating the area is well-known in the field and will not be elaborated here.

[0045] S204 defines the safe operating time window. The processing unit calculates the area safety threshold based on the maximum static polygon area where the robot is in contact with the ground at all links 35, combined with a safety margin ratio coefficient. The safety margin ratio coefficient is set to a range of 0.6 to 0.85 to ensure sufficient stability margin. The processing unit will support the area function. Area safety threshold Numerical comparisons are performed. Within the future prediction time domain, the system searches for continuous time segments where the area function calculation result is greater than or equal to the safety threshold. The system defines these continuous time segments as safe operation time windows. .

[0046] The processing unit further divides the safe operation time window interval The final cooperative control window is generated by intersecting the phase-locked window determined subsequently based on ground contact events. Only if the current moment is within the final coordinated control window During this period, the system allows the working arm to perform high-acceleration operations and allows the compensating arm to output feedforward compensation torque, as expressed mathematically below: ; The safe operation time window interval represents the robot having a supporting base surface sufficient to resist large inertial torque impacts during this period. The system uses this as the time boundary condition for the subsequent constraint of the working mechanism 2 to perform the operation.

[0047] See attached document Figure 6 The collaborative control method provided by this invention, after defining the safe operation time window, performs system-level functional characterization of the multiple mounted working mechanisms 2 for specific operation tasks, and pre-calculates the dynamic impact of the operation actions on the main body 1. Specifically, it includes the following role division and feedforward quantization steps: S205, Analyze the work instructions and classify the multi-arm roles. The system processing unit parses the received target grasping or contact work instructions and obtains the absolute coordinates of the target object. The processing unit combines the kinematic models of each working mechanism 2 to solve the inverse kinematics solution for reaching the target object. The system eliminates working mechanisms 2 that have no inverse kinematics solution or whose inverse kinematics solution is in a singular configuration, and obtains the set of reachable working mechanisms.

[0048] Within the set of reachable working mechanisms, the processing unit calculates the Euclidean distance between the multi-functional claw 27 at the end of each working mechanism 2 and the target object. In single-target grasping or single-point contact operation scenarios, the system divides the working mechanism 2 closest to the target object into the working arm and the remaining working mechanisms 2 into the compensation arm. In multi-point grasping or multi-arm collaborative operation scenarios, at least two working mechanisms 2 that meet the conditions of accessibility, obstacle avoidance, and load distribution are divided into a set of working arms, and the remaining working mechanisms 2 are divided into a set of compensating arms. The processing unit establishes the set of working arms and the set of compensating arms within the control architecture. The working arms are responsible for tracking instructions to complete terminal interaction tasks, while the compensating arms are responsible for swinging in the unconstrained space to generate inertial torques to counteract disturbances.

[0049] S206 generates the restricted movement trajectory of the boom. The processing unit determines the trajectory based on the safe operating time window. The start and end times of this time window interval are used as the time boundary conditions for the working arm's motion trajectory. Within these time boundary conditions, the system, combined with the coordinates of the target object, plans the target motion curve for the working arm's joint space. The processing unit reads the rated speed and peak torque parameters of the servo motors in the working mechanism 2 and sets the joint speed safety threshold and acceleration safety threshold. The processing unit performs time differentiation on the target motion curve to obtain the target joint positions of the working arm within each control cycle. Target joint velocity and target joint acceleration The parameters are then checked to see if they exceed the set safety threshold range. For the smooth interpolation planning of the spatial motion curve of the robotic arm joints, those skilled in the art can use the fifth-order polynomial interpolation method to write an algorithm. The trajectory planning process is a well-known technology in this field and will not be described in detail here.

[0050] S207, Quantifying the feedforward disturbance of the base caused by the operation motion. When the boom executes its trajectory or transports a load, changes in mass distribution and acceleration transmit a reaction torque to the main body 1. Based on the constructed multi-rigid-body dynamics model, the processing unit extracts the local parameter matrix characterizing the dynamic coupling relationship between the boom and the main body 1. The system, combined with the anticipated external load force, constructs a feedforward disturbance quantification equation and calculates the equivalent disturbance spinor transmitted from the operation motion to the center of mass of the main body 1. The equations are solved as follows: ; in, The coupling inertia matrix between the working arm and the main body 1 is... The corresponding coupled Coriolis force and centrifugal force matrix, The coupled gravity vector generated by the change in the mass distribution of the boom on the main body 1. The spatial force equivalent transformation matrix is ​​given by the coordinate system of the end effector of the boom to the coordinate system of the center of mass of the main body 1. This refers to the expected contact force vector generated by the interaction between the end effector multi-functional gripper 27 and the external load. This expected contact force vector is obtained by a force / torque sensor installed at the multi-functional gripper 27, or estimated by the target object's mass, gripping posture, operational trajectory acceleration, and a preset contact model.

[0051] Equivalent perturbation spinor It includes three-dimensional disturbance force components and three-dimensional disturbance torque components acting on the main body 1. The system uses this equivalent disturbance spinor as a feedforward compensation parameter to provide a quantitative numerical target for the inverse control of the subsequent compensation arm.

[0052] See attached document Figure 7The cooperative control method provided by this invention, after quantifying the base feedforward disturbance caused by the operation action, evaluates the ability of the compensation arm in a non-operational state to counteract the disturbance. When the force exertion capability of the current posture cannot meet the compensation requirements, the system triggers a configuration extension mechanism to increase the reverse torque output. Specifically, it includes the following evaluation and triggering steps: S301, Calculate the dynamic capability margin of the compensating arm. The system processing unit reads the joint positions of each working mechanism 2 in the compensating arm set and the upper limit of the rated output torque of the corresponding servo motor. Combining the current kinematic Jacobian matrix of the compensating arm, the processing unit uses the mapping relationship of the transpose of the Jacobian matrix to map the upper limit of the torque in the joint space to the center of mass of the main body 1, and calculates the maximum equivalent compensating spin that the compensating arm can produce under the current configuration.

[0053] Specifically, the processing unit constructs a feasible domain for the compensation torque using the upper limit of the torque of the servo motors at each joint of the compensating arm, the joint angle limit, and the joint speed limit as constraints, and then applies the three-dimensional disturbance torque. Find the maximum projection value of the feasible region in the opposite direction, and use the torque vector corresponding to the maximum projection value as the maximum equivalent compensation torque. .

[0054] To avoid dimensional conflicts caused by mixing three-dimensional force and three-dimensional torque, the processing unit extracts the three-dimensional torque component from the maximum equivalent compensation spinor, denoted as... The three-dimensional perturbation torque component is extracted from the equivalent perturbation spinor and denoted as... The processing unit will apply the maximum equivalent compensation torque. Modulus length and three-dimensional disturbance moment The dynamic capability margin coefficient is generated by performing a ratio calculation on the modulus length. The equations are solved as follows: ; in, This represents the operation of extracting the magnitude of a vector. This margin coefficient reflects the system's reserve capacity to withstand anticipated operational shocks under its current attitude.

[0055] S302, determine the margin status and trigger configuration switching. The system retrieves the preset margin safety threshold from memory. The safety margin threshold is set between 1.2 and 1.5 to provide a safety redundancy for handling sudden external collisions while mitigating operational disturbances. The processing unit will then use the dynamic capability margin coefficient. With margin safety threshold Numerical comparison is performed. When the dynamic capability margin coefficient is determined to be greater than or equal to the margin safety threshold, it indicates that the compensation arm has adjustment space, and the system maintains the zero-space motion planning logic; when the dynamic capability margin coefficient is determined to be less than the margin safety threshold, it indicates that the reverse force capability of the current posture is insufficient to offset external disturbances, and the processing unit generates a limit arm trigger signal.

[0056] The processing unit simultaneously calculates the predicted time of the most recent ground-lift event of the protective pad 36 based on the contact state prediction sequence. The calculation of the motion time required for the compensating arm to complete the configuration switch is based on the current joint position of the compensating arm, the target reconstructed pose, and the upper limit of joint velocity; when When the remaining time between the current moment and the current moment is greater than the sum of the action time required to complete the configuration switch and the preset safety time margin, the processing unit allows the compensating arm to perform the ultimate lever arm deployment action; when the remaining time is insufficient, the processing unit reduces the target acceleration of the working arm, delays the working arm action, or adjusts the gait phase of the motion mechanism 3. S303, plan the trajectory of the limit lever arm deployment. Upon receiving the limit lever arm trigger signal, the system reconstructs the kinematic solution objective of the compensating arm. The processing unit, based on the three-dimensional disturbance torque... The direction of the overturning moment is calculated on the horizontal plane, using the reverse vector direction required to counteract it. The processing unit searches for the limiting extension configuration within the workspace of the compensating arm, with the optimization objective of maximizing the projection distance of the compensating arm's center of mass in the aforementioned reverse vector direction.

[0057] The system controls the servo motors of each joint in the compensating arm to rotate in coordination, so that the multi-functional claw 27 extends away from the main body 1 and in a direction contrary to the disturbance trend, until it reaches the hardware interference limit or the preset kinematic boundary.

[0058] During the search for the ultimate extension configuration, the processing unit simultaneously verifies the collision relationships between the compensating arm and the working arm, main body 1, motion mechanism 3, the ground, and the target object, and uses the no-collision condition as the constraint condition for the configuration search. This extension action increases the length of the center of mass lever arm, and utilizes the mass distribution of the compensating arm itself to amplify the gravity compensation torque and the inertial torque generated by dynamic swinging.

[0059] For the optimization process of finding the extrema of a function in a confined space using the Lagrange multiplier method, those skilled in the art can write algorithms based on mathematical analysis theory. The extrema solution process is a well-known technique in this field and will not be elaborated upon here. The processing unit uses the calculated limit extension configuration as the initial candidate pose for reshaping the compensation arm configuration, and performs subsequent inertia tensor reshaping and gradient optimization operations near this initial candidate pose to obtain the target reshaping pose that takes into account the lever arm length, rotational inertia, and mechanical constraint.

[0060] The cooperative control method provided by this invention adjusts the system's mass distribution by changing the pose of the compensating arm when triggering the configuration extension mechanism or executing null space motion planning. The system uses a gradient optimization algorithm to reshape the inertia tensor of the compensating arm, enabling it to exhibit anti-tipping characteristics in the disturbed direction. Specifically, it includes the following tensor reshaping and gradient optimization steps: S304 constructs a pose-dependent inertia tensor model. The processing unit extracts the mass parameters and local rotational inertia of each link in the compensating arm. This is combined with the current joint position variables of the compensating arm. The processing unit calculates the position vector of each link's center of mass relative to the center of mass of the main body 1, and extracts the rotation transformation matrix from the coordinate system of each link to the coordinate system of the base of the main body 1. Based on this rotation transformation matrix and the parallel axis theorem, the processing unit calculates the global inertia tensor matrix of the compensating arm equivalent to the center of mass of the main body 1. This global inertia tensor matrix is ​​a matrix function of the joint position variables, denoted as... This matrix characterizes the effect of the compensating arm on the rotational characteristics of the main body 1 under different configurations.

[0061] S305 defines the inertial performance index in the direction of force application. The processing unit obtains the three-dimensional disturbance moment from the previous solution, normalizes it, and extracts the unit direction vector representing the disturbance axis. The system aims to maximize the rotational inertia of the compensating arm along the disturbance axis, and constructs an inertial performance index function. The equations are solved as follows: ; Here, the superscript T indicates the matrix transpose operation. This performance index function projects the tensor matrix onto the force direction, quantifying the resistance of the compensating arm to operational disturbances.

[0062] S306 executes gradient iterative optimization calculation. To solve for the joint position variables that maximize the inertial performance index function, the processing unit uses a gradient optimization algorithm for numerical calculation. The processing unit calculates the performance index function with respect to the joint position variables. The partial derivative vector is used to obtain the search gradient of the objective function in the current configuration. .

[0063] The system extracts the preset iteration step size parameters from the memory. The step size parameter is set based on the calculation frequency and numerical convergence accuracy requirements of the control unit, and its value range is set to 0.01 to 0.05 radians / time. The processing unit constructs the iterative update equation for the joint position variables, and the solution equation is as follows: ; in, This is a label for the current iteration count. The processing unit performs iterative calculations, checking after each iteration whether the configuration touches the joint limit boundaries or causes link interference. If the joint position variables calculated iteratively... If the target variable exceeds the mechanical limit boundary, the processing unit truncates the variable value of the out-of-bounds joint to the corresponding limit boundary value, ensuring that the target variable is within the physically feasible region. The system extracts a preset convergence threshold, which is set to 0.0001.

[0064] When the magnitude of the gradient is less than the convergence threshold, or when the number of iterations reaches the set upper limit of the loop, the processing unit terminates the optimization process. The system uses the finally converged joint position variables as the target to reshape the pose. For the solution of the partial derivative matrix and the detection of link interference in the gradient calculation, those skilled in the art can write algorithms based on computational geometry. The partial derivative calculation and interference detection process are well-known technologies in this field and will not be described in detail here.

[0065] S307, issue a reshaping command and execute pose switching. The processing unit converts the calculated target reshaping pose into target position commands for each joint motor. The control system issues this command to the corresponding working mechanism 2 in the compensating arm set. The driver drives the motor to move to the target reshaping pose according to the command. This process increases the robot's equivalent rotational inertia in the disturbed direction by actively changing the structural mass distribution, thereby increasing the system's stability against overturning moments.

[0066] See attached document Figure 8 The cooperative control method provided by this invention, after acquiring the working trajectory and the pose of the compensation target, synthesizes the underlying control commands through a zero-space cascaded projection algorithm. This algorithm isolates interference between different tasks, ensuring that the compensation arm does not affect the end-effector trajectory when performing tensor reshaping or anti-tipping actions. Specifically, it includes the following decoupling control law construction steps: S308 defines system task priorities and matrix extraction. The system processing unit establishes the task control hierarchy based on the physical division of labor of working mechanism 2. The tracking of the target object by the working arm is defined as a high-priority primary task, while the pose reshaping and reverse force application of the compensating arm are defined as low-priority secondary tasks.

[0067] The processing unit reads the global joint state parameters of the current multi-arm system and constructs a generalized joint space including all working mechanisms 2. The processing unit extracts the kinematic Jacobian matrix corresponding to the main task and extends it to the generalized joint space dimension to match the global degrees of freedom, denoted as . This matrix represents the mapping relationship between the generalized joint space velocity and the end-effector operation space velocity.

[0068] S309, Construct the null space projection operator. The processing unit calculates the Jacobian matrix. The pseudo-inverse matrix is ​​given. To avoid direct inversion when the robotic arm is in a singular configuration, which could lead to divergence in the output command, the processing unit uses damped least squares method to calculate this pseudo-inverse matrix, denoted as . The preset damping factor is extracted from the memory, with a value range of 0.01 to 0.1, to adjust tracking accuracy and numerical stability. The processing unit constructs a null projection operator based on this pseudo-inverse matrix. The equations are solved as follows: ; in, This is the identity matrix, consistent with the global system's degree of freedom dimension. The projection operator is essentially an orthogonal mapping filter, used to filter and remove components from the input vector that cause the end effector motion of the working arm.

[0069] S310 calculates the independent control variables for primary and secondary tasks. The processing unit combines the target trajectory with the current joint state feedback to calculate the primary joint acceleration command required to complete the primary task. Zeros are then padded in the dimensions corresponding to non-operating arms to expand it into a global command vector, denoted as... The processing unit combines the target reconstructed pose with the current state of the compensating arm to calculate the secondary joint acceleration commands required to complete the secondary task. Similarly, zeros are padded in the dimensions corresponding to the non-compensating arms, expanding this into a global command vector, denoted as... .

[0070] For closed-loop acceleration command feedback calculation based on position and velocity errors, those skilled in the art can use proportional-derivative control algorithms to write programs. The error feedback solution process is a well-known technology in this field and will not be described in detail here.

[0071] S311, synthesize and execute the cascaded decoupling control law. The processing unit uses the null-space projection operator to map the secondary joint acceleration commands, superimposes them onto the primary joint acceleration commands, and synthesizes a global decoupling control law command vector. The equations are solved as follows: ; After mapping using this equation, the motion generated by the secondary task is constrained within the null space of the primary task. The processing unit will decouple the control law command vector. The system inputs a preset inverse dynamics model to calculate the required driving torque for each joint. The control system then sends this driving torque command to the drivers of each working mechanism 2. The drivers drive the servo motors to output the corresponding torque. This process ensures the synchronous arrival of the working arm at the target coordinates, drives the compensation arm to complete the anti-tipping posture switching, and achieves decoupled control of the multi-arm system functions.

[0072] The cooperative control method provided by this invention transforms continuous gait motion into a standardized phase reference when the robot is in a mobile operation state. The system provides a synchronous triggering sequence for the aforementioned tensor reshaping and motion decoupling by defining a support window and performing time warping mapping. Specifically, it includes the following definition and mapping steps: S401, Extract gait phase transition event. The processing unit reads the contact force sensor data at the protective pad 36 in the motion mechanism 3. The processing unit extracts a preset contact force threshold, which is set according to the weight of the motion mechanism 3 and the sensor range, with a value range of 30 to 50 Newtons.

[0073] The processing unit compares the real-time feedback value from the sensor with the threshold. When the feedback value is greater than the threshold, it determines that the corresponding motion mechanism 3 has experienced a ground contact event, and records the current moment as the start of the gait cycle. When the feedback value is less than the threshold, it determines that a ground lift event has occurred. Based on this logic, the processing unit obtains the phase transition timestamp of each motion mechanism 3 to determine the physical support state of the base.

[0074] S402, Define the gait phase-locked window. During movement, the system establishes the support timing to resist disturbances. The processing unit calculates the overlapping time period when the protective pads 36 in each motion mechanism 3 are in ground contact state based on the time sequence of ground contact events and ground lift events. The system extracts the time period when the number of protective pads 36 that reach the preset support number simultaneously touches the ground, and defines it as the gait phase-locked window.

[0075] To ensure the formation of a two-dimensional geometric surface capable of effective area calculation, the preset number of supports is set to be greater than or equal to 3 for multi-legged robot configurations such as quadrupeds or hexapods. When the number of protective pads 36 in contact with the ground is less than 3, the system does not perform area calculation of the support polygon, but instead classifies the current time period as a non-area-stable support period, restricting the working arm from performing large-scale acceleration movements. Within this window, the support polygon formed by the system has the mechanical basis for performing posture reshaping and anti-tipping force generation. The processing unit generates a window opening signal, instructing the working mechanism 2 to perform cooperative operation actions within this window time period.

[0076] S403, performs time warping mapping. Due to terrain undulations or changes in speed commands, gait cycle duration fluctuates. To eliminate the interference of time fluctuations on the control law, the processing unit uses a time warping algorithm to map absolute time into a normalized phase variable. The processing unit obtains the time difference between the current absolute time and the start time of the gait cycle, denoted as . And read the prediction cycle time issued by the underlying planner, denoted as To prevent calculation divergence, a preset lower limit for the period time is extracted, which is set to 0.2 seconds. If the predicted period time is lower than this lower limit, the processing unit truncates it to this lower limit. The processing unit combines the above time difference with the predicted period time to calculate the phase variable. The equations are solved as follows: ; The processing unit truncates the calculation results by an upper limit, restricting the phase variable. The value of is between zero and one. This mapping operation transforms the non-uniform time sequence into a unified phase space reference. For continuous trajectory interpolation based on phase variables, those skilled in the art can write algorithms based on spline curve theory, and the trajectory interpolation process is a well-known technique in the field, which will not be elaborated here. The processing unit uses the calculated phase variables as a unified time reference system for the underlying motion control.

[0077] The cooperative control method provided by this invention constructs a feedforward control law using the reshaped inertia tensor after defining the gait phase-locked window. The system intervenes before errors occur by calculating the expected disturbance torque and applying a counter-torque in advance. Specifically, it includes the following feedforward compensation execution steps: S404, determine the compensation trigger condition. The processing unit monitors the status signal of the gait phase-locked window. When a window open signal is detected, and the current time is within the final cooperative control window... Within this timeframe, the system is determined to be in a stable support state, and the feedforward compensation mechanism is activated. The processing unit reads the phase variable output by the time warping mapping and uses it as a synchronization reference for motion trajectory generation and torque distribution, providing a unified timing reference for the actions of each working mechanism 2.

[0078] S405 calculates the dynamic feedforward torque. The processing unit extracts the reshaped global inertia tensor matrix. The processing unit reads the target motion trajectory and load mass of the boom and inputs them into the system's preset multibody dynamics model to calculate the disturbance torque generated by the boom's motion on the center of mass of the main body 1. Based on the angular momentum theorem, the processing unit sets the body's disturbance resistance angular acceleration requirement for maintaining the current posture, denoted as . The processing unit combines the global inertia tensor matrix with the disturbance rejection angular acceleration requirements to calculate the main compensation torque needed to counteract the disturbance. The equations are solved as follows: ; The processing unit extracts the current kinematic Jacobian matrix of the compensating arm, and uses the transpose of this matrix to map the main compensating torque in the operation space to the joint feedforward torque vector of the compensating arm, denoted as . For torque prediction and Jacobian transpose mapping based on multibody dynamics models, those skilled in the art can write programs based on robot mechanics theory. The spatial mapping solution process is a well-known technology in this field and will not be elaborated here.

[0079] S406, synthesize the control torque and issue it for execution. The processing unit obtains the feedback driving torque calculated from the preceding inverse dynamics model, denoted as... The processing unit superimposes the joint feedforward torque vector onto the feedback drive torque to synthesize a global control torque for the underlying drive. The equations are solved as follows: ; The control system sends the global control torque to the driver of the corresponding working mechanism 2 in the compensating arm set. The driver drives the servo motor to output torque according to the global control torque. This mechanism, combined with the reshaped inertia and feedforward algorithm, intervenes with force before attitude deviation occurs, increasing the system's stability against operational disturbances.

[0080] S407 executes safety degradation control. The processing unit continuously monitors the zero-moment point location and dynamic capability margin coefficient during the control cycle. Final collaborative control window and the progress of the compensating arm configuration switch. When it is determined that the current zero moment point falls outside the support polygon, or the dynamic capability margin coefficient... When the velocity falls below the safety margin threshold and the compensating arm cannot complete its configuration remodeling before the predicted takeoff time, the processing unit reduces the target joint acceleration of the working arm, suspends the end-effector contact action of the working arm, or adjusts the gait phase of motion mechanism 3 to expand the support area; when the zero-moment point re-enters the support polygon and the dynamic capability margin coefficient... Once the system recovers to above the safety margin threshold, it resumes normal collaborative control procedures.

[0081] Working principle: At the start of operation, the probe 4 installed at the front end of the main body 1 detects and locates the environment and target objects in front. Based on the detection information, the motion mechanism 3 fixedly connected to the bottom of the main body 1 begins to operate. Specifically, a frame 32 is fixedly connected to the outside of the main board 31, and a drive source drives the rotating shaft 33, which is rotated and connected to the outside of the frame 32, to rotate. Since the long plate 34 is installed outside the rotating shaft 33, the rotation of the rotating shaft 33 causes the long plate 34 to swing back and forth. Simultaneously, the connecting rod 35, which is rotated and connected to the bottom of the long plate 34, is activated in conjunction with the rod, simulating the stepping motion of a bionic leg. The protective pad 36, which is wrapped around the bottom of the connecting rod 35, continuously contacts the ground and provides cushioning, thereby driving the entire main body 1 to move smoothly to the target position.

[0082] Once the robot reaches the designated work area, multiple working mechanisms 2 fixedly connected to the top of the main body 1 begin to work collaboratively. First, the mounting plate 22, which is rotatably connected to the top of the mounting base 21, rotates horizontally, thereby adjusting the overall working orientation of the robotic arm. Subsequently, since the top of the mounting plate 22 is rotatably connected to the connecting arm 23 via the first pivot 24, the connecting arm 23 tilts and swings around the first pivot 24 to adjust the span and height of the working arm.

[0083] During this process, the motor 25 located at the top of the connecting arm 23 is started. Since the output end of the motor 25 is fixedly connected to the second rotating shaft 28, the motor 25 drives the second rotating shaft 28 to rotate, which in turn drives the working arm 26, which is rotatably connected to the outside of the motor 25, to rotate relative to the target, thereby realizing the flexible flexion and extension of the end joint of the robotic arm and accurately positioning the working arm 26 to the target location.

[0084] Finally, the multi-functional claw 27 installed at the bottom of the working arm 26 performs opening and closing actions to grasp, transport, or perform construction operations on the target object. During these operations, the processing unit within the main board 31 extracts gait phase-locked windows with a support number greater than or equal to three, and quantifies operational disturbances using a spatial force transformation matrix. When the assessment indicates insufficient anti-overturning compensation capability, and the remaining time off the ground exceeds the sum of the action time required to complete the configuration switch and the safety margin, the system triggers the working mechanism 2, which is in a non-operational state, to extend its ultimate lever arm and output a reverse torque. Multiple working mechanisms 2 repeat the above actions on the top of the main body 1, and coordinate with the chassis dynamic gait for feedforward compensation, achieving stable collaborative work of a biomimetic multi-arm system in complex environments.

Claims

1. A cooperative control method for a biomimetic multi-armed robot, characterized in that, include: Construct a dynamic model that includes the main body (1), the motion mechanism (3) and multiple working mechanisms (2); Extract the motion cycle data of the motion mechanism (3) to construct a dynamic support polygon, and combine it with the operation instructions received by the working mechanism (2) to deduce the expected disturbance torque generated by the operation instructions on the main body (1) based on the dynamic model; According to the work instructions, the multiple working mechanisms (2) are divided into working arms and compensation arms. When it is determined that the compensation capability for the expected disturbance torque is insufficient, an extension command is issued to the compensation arm to control the compensation arm to convert to an extended configuration. Based on the area of ​​the dynamic support polygon, the time window is defined. When the working arm is disturbed during the time window, the compensation arm in the extended configuration outputs a reverse joint acceleration to generate a reaction torque to counteract the dynamic offset of the main body (1).

2. The cooperative control method for a biomimetic multi-armed robot according to claim 1, characterized in that, The construction of the dynamic model, which includes the main body (1), the motion mechanism (3), and multiple working mechanisms (2), further includes the step of solving the spatial coordinates of the actual zero-moment point, specifically including: Obtain the pose state vector of the main body (1), the bottom driving vector of the motion mechanism (3) and the independent joint vector of the working mechanism (2), and concatenate the pose state vector, the bottom driving vector and the independent joint vector to generate the system global generalized coordinate vector; A Lagrangian function is established based on the physical parameters and real-time motion speed of each link. The corresponding parameter matrix is ​​extracted based on the Lagrangian function. The multi-rigid-body dynamic equation is generated by combining the contact constraint mapping model applied by the ground to the motion mechanism (3) as the dynamic model. Based on the dynamic model, the spatial acceleration of the machine's center of mass and the rate of change of spatial angular momentum around the center of mass are calculated. The two-dimensional plane coordinates of the actual zero-moment point in the current state are deduced, and it is determined whether the two-dimensional plane coordinates fall within the range of the dynamic support polygon.

3. The cooperative control method for a biomimetic multi-armed robot according to claim 1, characterized in that, The step of extracting the motion cycle data of the motion mechanism (3) to construct a dynamic support polygon and defining the time window includes: The end of the motion mechanism (3) is provided with a protective pad (36). Based on the angular displacement and angular velocity parameters of the motion mechanism (3), the height trajectory of the protective pad (36) is deduced, and a contact state prediction sequence of the protective pad (36) changing with time is generated. The ground contact state parameters in the contact state prediction sequence are extracted and projected onto the horizontal plane as the ground position data. The geometric area of ​​the region enclosed by the two-dimensional coordinate point set is calculated using the convex hull algorithm, and a support area function that changes with time is generated. The supporting area function is compared with the area safety threshold. In the future prediction time domain, continuous time segments in which the area calculation result is greater than or equal to the area safety threshold are extracted, and a safe operation time window interval is generated as the time window.

4. The cooperative control method for a biomimetic multi-armed robot according to claim 2, characterized in that, The process of dividing the multiple working mechanisms (2) into working arms and compensation arms according to the work instructions, and issuing an extension instruction to the compensation arms when it is determined that the compensation capability for the expected disturbance torque is insufficient, includes: Calculate the Euclidean distance between the end of each working mechanism (2) and the target object, and solve the inverse kinematic solution to reach the target object by combining the kinematic model of each working mechanism (2) to generate a set of reachable working mechanisms. In the set of reachable working mechanisms, the working mechanism (2) closest to the target object is divided into the working arm, and the working mechanism (2) that is not the working arm is divided into the compensation arm. Based on the dynamic model, the coupling parameter matrix is ​​extracted, and the equivalent disturbance spinor transmitted by the operation action to the centroid of the main body (1) is calculated as the expected disturbance torque. The maximum equivalent compensation torque of the compensation arm under the current configuration is calculated using the Jacobian matrix transpose mapping. The dynamic capability margin coefficient is generated by calculating the ratio of the modulus of the maximum equivalent compensation torque to the modulus of the expected disturbance torque. When the dynamic capability margin coefficient is less than the set margin safety threshold, and the remaining time of the predicted takeoff time is greater than the sum of the time required for configuration switching and the preset safety time margin, the compensation capability is determined to be insufficient.

5. The cooperative control method for a biomimetic multi-armed robot according to claim 1, characterized in that, The control of the compensation arm to convert to the extended configuration includes: Extract the mass parameters and local rotational inertia of each link in the compensation arm, and calculate the global inertia tensor matrix of the compensation arm equivalent to the centroid of the main body (1); The global inertia tensor matrix is ​​projected onto the force direction, and an inertial performance index function is constructed based on the rotational inertia of the compensating arm in the disturbance axis. The gradient optimization algorithm is used to calculate the partial derivative vector of the inertial performance index function with respect to the joint position variables. An iterative update operation with mechanical limit truncation is performed. The joint position variable corresponding to the gradient magnitude being less than the convergence threshold is used as the target repositioning and sent to the compensation arm for execution, so that the compensation arm is converted into the extended configuration.

6. The cooperative control method for a biomimetic multi-armed robot according to claim 5, characterized in that, The process of controlling the compensation arm to convert to an extended configuration also includes a step of using a zero-space projection control law to impose constraints without interfering with the working arm, specifically including: Extract the kinematic Jacobian matrix corresponding to the main task, calculate the pseudo-inverse matrix of the kinematic Jacobian matrix using the damped least squares method, and construct the null space projection operator based on the pseudo-inverse matrix and the identity matrix. The main joint acceleration command is calculated by combining the target trajectory contained in the operation command with the current joint state feedback. The secondary joint acceleration command is calculated by combining the target reshaped pose with the current joint state feedback of the compensating arm. The main joint acceleration command and the secondary joint acceleration command are then extended into a global command vector. The secondary joint acceleration command is orthogonally mapped and filtered using the null projection operator. The mapped and filtered secondary joint acceleration command is superimposed on the primary joint acceleration command to synthesize a decoupled control law command vector. The decoupled control law command vector is then input into the inverse dynamics model to calculate the driving torque for control.

7. The cooperative control method for a biomimetic multi-armed robot according to claim 5, characterized in that, When the working arm movement causes a disturbance within the time window, the compensating arm in the extended configuration outputs a reverse joint acceleration, including: The time period during which the motion mechanism (3) meets the preset number of supports and touches the ground is defined as the gait phase-locked window. When the system is in the gait phase-locked window and within the time window, the feedforward compensation mechanism is activated. Obtain the time difference between the current absolute time and the start time of the gait cycle, calculate the quotient of the time difference and the predicted cycle time and truncate it, and map it to generate a normalized phase variable as a motion trajectory synchronization reference. The main body compensation torque is calculated by combining the global inertia tensor matrix and the disturbance rejection angular acceleration requirement. The main body compensation torque is mapped to the joint feedforward torque vector of the compensation arm by transposing the Jacobian matrix. The joint feedforward torque vector is superimposed on the feedback driving torque calculated by the inverse dynamics model to synthesize the global control torque and send it to the compensation arm.

8. The cooperative control method for a biomimetic multi-armed robot according to claim 4, characterized in that, After generating a reaction torque to counteract the dynamic offset experienced by the main body (1), the process further includes the step of performing safety degradation control: Continuously monitor the actual zero-moment point spatial coordinates and the configuration state of the compensation arm; When it is determined that the actual zero-moment point spatial coordinates fall outside the range of the dynamic support polygon, or when it is determined that the compensation arm cannot complete the configuration reshaping before the predicted ground lift-off time, the target joint acceleration of the working arm is reduced, the end contact action is paused, or the gait phase of the motion mechanism (3) is adjusted. When the actual zero-moment point spatial coordinates re-enter the range of the dynamic support polygon and the dynamic capability margin coefficient is determined to have recovered to above the margin safety threshold, the normal collaborative control process is restored.

9. A biomimetic multi-armed robot, applied to the cooperative control method of the biomimetic multi-armed robot according to any one of claims 1-8, characterized in that, Includes a main body (1), with multiple working mechanisms (2) fixedly connected to the top of the main body (1), a motion mechanism (3) fixedly connected to the bottom of the main body (1), and a probe head (4) installed at the front end of the main body (1). The working mechanism (2) includes a mounting base (21), a mounting plate (22) is rotatably connected to the top of the mounting base (21), a connecting arm (23) is rotatably connected to the top of the mounting plate (22) via a first rotating shaft (24), a second rotating shaft (28) is provided on the top of the connecting arm (23), the rotating shaft (28) is provided at the output end of the motor (25), a working arm (26) is rotatably connected to the outside of the motor (25), and a multi-functional claw (27) is installed at the bottom of the working arm (26).

10. A biomimetic multi-armed robot according to claim 9, characterized in that, The motion mechanism (3) includes a main board (31), a frame (32) is fixedly connected to the outside of the main board (31), a rotating shaft (33) is rotatably connected to the outside of the frame (32), a long plate (34) is installed on the outside of the rotating shaft (33), a connecting rod (35) is rotatably connected to the bottom of the long plate (34), and a protective pad (36) is wrapped around the bottom of the connecting rod (35).

Images