A multi-arm body intelligent robot body stability control method and system

Abstract

The present application relates to the technical field of industrial robots, in particular to a multi-arm body intelligent robot body stability control method and system, which realizes the active perception of the stability trend of the chassis in advance by predicting the limit reaction force generated by the mechanical arm on the future mobile platform and calculating the stability index; on this basis, the impedance parameter constraint boundary is dynamically contracted according to the stability index, and the constraint is directly embedded into the optimization solving process, so that the impedance parameter is adaptively adjusted with the stability margin; when the stability is sufficient, the boundary is released to ensure the force tracking accuracy, and when the stability is tight, the boundary is actively contracted to reduce the reaction force impact, so that the high-precision force control and operation safety are considered.

Description

Technical Field

[0001] This invention relates to the field of industrial robot technology, and in particular to a method and system for controlling the stability of a multi-armed intelligent robot. Background Technology

[0002] Embodied Intelligent Robots (WILOs) consist of a mobile platform and a multi-jointed robotic arm, possessing both wide-range mobility and precise manipulation capabilities. In recent years, they have been increasingly widely used in physical interaction scenarios such as industrial assembly, polishing, and material handling. In these tasks, the robotic arm's end effector needs to continuously interact with the environment in terms of force and position; therefore, impedance control becomes a key technology for achieving compliant force tracking. Existing impedance control methods typically require pre-tuning of stiffness and damping parameters for specific tasks, which is time-consuming and difficult to adapt to changes in working conditions.

[0003] To address this issue, existing technologies have proposed an online impedance parameter tuning method based on a combination of learning and optimization, which achieves online adaptive updating of impedance parameters to a certain extent. Furthermore, regarding the stability assurance of embodied intelligent robots, traditional methods often employ offline pose optimization or quasi-static planning strategies, pre-calculating the zero-moment point positions under various robot arm configurations to ensure chassis balance during operation.

[0004] However, the aforementioned existing technologies still have the following drawbacks: First, existing online impedance parameter tuning methods generally treat the mobile platform as a fixed base, failing to consider the impact of the reaction force generated by the robot arm's movement on the dynamic stability of the chassis. When the robot arm moves rapidly in an extended configuration or bears a large external load, the reaction force will significantly change the ZMP position of the mobile platform, potentially causing chassis swaying or even overturning. Traditional impedance control methods lack online sensing and active constraint capabilities for this. Second, existing stability assurance methods are mostly offline planning or quasi-static analysis, lacking the ability to predict the real-time decay of stability margin caused by the dynamic movement of the robot arm. In particular, impedance parameter optimization and mobile platform stability control belong to two independent processing loops, lacking information interaction and collaborative decision-making mechanisms between them, and cannot adjust impedance characteristics in real time according to the dynamic changes in the current stability margin.

[0005] In summary, existing methods have not yet solved the problem of coordinated control between online optimization of impedance parameters and prediction of dynamic stability of the mobile platform, making it difficult to balance force control accuracy and operational safety. Summary of the Invention

[0006] In view of this, the purpose of the present invention is to provide a method and system for controlling the stability of a multi-armed intelligent robot.

[0007] In a first aspect, embodiments of the present invention provide a method for controlling the stability of a multi-armed intelligent robot. The multi-armed intelligent robot includes a mobile platform and at least two robotic arms disposed on the mobile platform. The method includes: Obtain the current motion state information of the robotic arm; Based on the current motion status information, predict the ultimate reaction force that the robotic arm will generate on the mobile platform within a future time window; Based on the ultimate reaction force information, calculate the stability index characterizing the current stability of the mobile platform; Based on the stability index, the constraint boundary of the robot arm's impedance parameters is determined, and the constraint boundary shrinks as the stability index decreases. The optimal impedance parameters are obtained by using the desired motion trajectory and desired contact force as control objectives and the constraint boundary of the impedance parameters as optimization constraints. Based on the optimal impedance parameters, the robotic arm is controlled to perform motion output.

[0008] In conjunction with the first aspect, the current motion state information includes the current joint angle vector and joint angular velocity vector of the robotic arm; Based on the current motion state information, the steps for predicting the ultimate reaction force that the robotic arm will exert on the mobile platform within a future time window include: Use the joint angle vector and the joint angular velocity vector as a joint query index; The limit reaction force boundary corresponding to the current motion state is obtained by searching the limit reachability set lookup table using the joint query index. The limit reachability lookup table uses the sampling points of joint angle and angular velocity as indexes and the coordinates of the convex hull of the limit reaction force corresponding to each sampling point as values; the limit reaction force boundary is the maximum range of the reaction force that each robotic arm end effector can generate on the mobile platform within a future time window.

[0009] In conjunction with the first aspect, the limit reachable set lookup table is pre-built offline in the following way: The joint angles and joint angular velocities of each robotic arm of the multi-armed intelligent robot are jointly discretized and sampled to form a joint sampling point set; For each sampling point in the joint sampling point set, with the joint torque allowable boundary, joint angle allowable boundary, and joint angular velocity allowable boundary as constraints, and with the optimization objective of maximizing the amplitude of the total reaction force generated by multiple robotic arm ends on the mobile platform within a future time window, the optimal control problem is solved to obtain the limit convex hull range of the total reaction force corresponding to the sampling point in three-dimensional space. Extract the vertex coordinates of the convex hull range, and store the vertex coordinates as a limit reachable set lookup table using the combined vector of joint angles and angular velocities as an index.

[0010] In conjunction with the first aspect, the steps for calculating a stability index characterizing the current stability of the mobile platform based on the ultimate reaction force information include: Determine the supporting polygon based on the grounding profile of the mobile platform; Based on the limit reaction force boundary of each robotic arm, the offset of the zero moment point caused by the reaction force of each robotic arm is calculated according to the zero moment point theory. The zero-moment point offsets of each robotic arm are superimposed and combined with the zero-moment point position of the mobile platform itself to obtain the comprehensive zero-moment point position under extreme conditions. Calculate the minimum distance from the location of the comprehensive zero moment point to the boundary of the supporting polygon, and use the minimum distance as a stability index.

[0011] In conjunction with the first aspect, the steps for determining the constraint boundaries of the robot arm's impedance parameters based on stability indices include: Based on the comparison between stability index and preset threshold, the current region and its stiffness scaling factor are determined. Multiply the nominal stiffness upper limit matrix by the stiffness scaling factor to obtain the stiffness upper limit matrix for the current control cycle; and multiply the nominal damping upper limit matrix by the stiffness scaling factor to obtain the damping upper limit matrix for the current control cycle. The upper limit matrix of stiffness and the upper limit matrix of damping are used as the constraint boundaries of the impedance parameters.

[0012] In conjunction with the first aspect, the steps for determining the current region and its stiffness scaling factor based on the comparison between the stability index and a preset threshold include: If the stability index is greater than the first threshold, it is determined to be in the ample zone, and the stiffness scaling factor is set to the first value. If the stability index is greater than the first threshold and less than or equal to the second threshold, it is determined to be in the warning zone. The stiffness scaling factor is calculated linearly according to the relative position of the stability index between the first and second thresholds. The stiffness scaling factor increases as the stability index increases. If the stability index is less than or equal to the first threshold, it is determined to be in the danger zone, and the stiffness scaling factor is set to the second value, which is less than the first value.

[0013] Combining the first aspect, with the desired motion trajectory and desired contact force as control objectives, and the constraint boundary of the impedance parameter as optimization constraint, the steps to solve the optimization problem and obtain the optimal impedance parameter include: Obtain the desired motion trajectory and desired contact force. The desired motion trajectory includes the desired end position and desired end velocity. A cost function for a quadratic programming optimization problem is constructed with the goal of minimizing the weighted sum of squares of force tracking error, motion trajectory tracking error, and the deviation of impedance parameters from their nominal values. The constraints for the quadratic programming optimization problem are constructed by using the upper bound matrix of stiffness and the upper bound matrix of damping as upper bound constraints for impedance parameters, the physically permissible minimum stiffness and minimum damping as lower bound constraints for impedance parameters, and the positive definiteness condition and the passivity condition of the system as feasibility constraints. Solve for the stiffness and damping matrices that minimize the cost function under constraints, and output them as the optimal impedance parameters.

[0014] In conjunction with the first aspect, the steps for controlling the robotic arm to execute motion output based on the optimal impedance parameters include: The upper optimization layer runs at the first preset frequency and outputs the current optimal impedance parameters based on the current stability index, the current impedance parameter constraint boundary, and the solution of the current optimization problem. The middle impedance control layer operates at a second preset frequency, and executes the Cartesian space impedance control law based on the current stiffness matrix and the current damping matrix in the current optimal impedance parameters to calculate the desired end force; wherein, the second preset frequency is higher than the first preset frequency; The bottom-level torque control layer operates at a third preset frequency, and calculates the driving torque of each joint motor according to the expected end force through the joint torque control algorithm; wherein, the third preset frequency is higher than the second preset frequency; For each robotic arm, the drive arm is used to execute motion output with the corresponding drive torque.

[0015] Secondly, this application also provides a stability control system for a multi-armed intelligent robot. The multi-armed intelligent robot includes a mobile platform and at least two robotic arms mounted on the mobile platform. The system includes: The stability prediction module is used to acquire the current motion state information of the robotic arm, predict the ultimate reaction force information of the robotic arm on the mobile platform within a future time window based on the current motion state information, and calculate the stability index characterizing the current stability of the mobile platform based on the ultimate reaction force information. The impedance parameter optimization module is used to determine the constraint boundary of the robot arm's impedance parameter based on the stability index. The constraint boundary shrinks as the stability index decreases. The module solves the optimization problem with the desired motion trajectory and desired contact force as control objectives and the constraint boundary of the impedance parameter as optimization constraints to obtain the optimal impedance parameter. The hierarchical control execution module is used to control the robotic arm to perform motion output based on the optimal impedance parameters.

[0016] In conjunction with the second aspect, the hierarchical control execution module includes: The upper-level optimization layer controller is used to operate at the first preset frequency, perform optimization solutions based on the current stability index and the current impedance parameter constraint boundary, and output the current optimal impedance parameter. The middle impedance control layer controller is used to operate at a second preset frequency. It executes the Cartesian space impedance control law based on the stiffness matrix and damping matrix in the current optimal impedance parameters and calculates the desired end force. The second preset frequency is higher than the first preset frequency. The bottom-level torque control layer controller is used to operate at a third preset frequency. Based on the desired end force, it calculates the driving torque of each joint motor through the joint torque control algorithm and drives each robotic arm to perform motion output. The third preset frequency is higher than the second preset frequency.

[0017] Thirdly, this application provides an electronic device, which includes a memory and a processor. The memory stores a computer program, and the processor runs the computer program to cause the electronic device to perform the above-described method.

[0018] Fourthly, this application provides a readable storage medium storing computer program instructions, which are read and executed by a processor to perform the above-described method.

[0019] The embodiments of the present invention bring the following beneficial effects: This application provides a method and system for controlling the stability of a multi-armed intelligent robot. The multi-armed intelligent robot includes a mobile platform and at least two robotic arms mounted on the mobile platform. The method includes: acquiring the current motion state information of the robotic arms; predicting the ultimate reaction force information of the robotic arms on the mobile platform within a future time window based on the current motion state information; calculating a stability index characterizing the current stability of the mobile platform based on the ultimate reaction force information; determining the constraint boundary of the impedance parameters of the robotic arms based on the stability index, wherein the constraint boundary shrinks as the stability index decreases; optimizing the solution with the desired motion trajectory and desired contact force as control targets and the constraint boundary of the impedance parameters as optimization constraints to obtain the optimal impedance parameters; and controlling the robotic arms to perform motion output based on the optimal impedance parameters.

[0020] This application achieves proactive pre-emptive perception of chassis stability trends by predicting the ultimate reaction force of the robotic arm on the mobile platform within a future time window and calculating stability indices. Based on this, the impedance parameter constraint boundary is dynamically contracted according to the stability index, and this constraint is directly embedded into the optimization solution process, allowing the impedance parameter to adaptively adjust with the stability margin. When stability is sufficient, the constraint boundary is loosened to ensure force tracking accuracy; when stability becomes tight, the boundary is actively contracted to reduce the impact of reaction forces, thus balancing high-precision force control and operational safety. Simultaneously, through the joint prediction and superposition processing of multi-arm reaction forces, the adverse effects of multi-arm coupled motion on chassis stability are reduced.

[0021] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention are realized and obtained in accordance with the structures particularly pointed out in the description, claims and drawings.

[0022] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0023] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0024] Figure 1 A flowchart illustrating a method for controlling the stability of a multi-armed intelligent robot body, provided in an embodiment of the present invention; Figure 2 A schematic diagram illustrating the principle of the method provided in the embodiments of the present invention; Figure 3 This is a schematic diagram illustrating the principle of limit reachability set prediction in the method provided in this embodiment of the invention; Figure 4 This is a schematic diagram illustrating the variation curve of the stiffness upper limit matrix with the predicted stability margin in the method provided by the embodiments of the present invention; Figure 5 This is a schematic diagram illustrating the adaptive partitioning adjustment principle in the method provided in this embodiment of the invention; Figure 6 This is a schematic diagram of a three-layer hierarchical control architecture in the method provided in the embodiments of the present invention; Figure 7 This is a schematic diagram of the threshold adaptive adjustment mechanism in the method provided in the embodiments of the present invention. Detailed Implementation

[0025] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions 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, 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.

[0026] To facilitate understanding of this embodiment, the technical terms used in this application will be briefly introduced below.

[0027] The positive definiteness condition requires that both the stiffness and damping matrices in the impedance parameters be positive definite matrices, meaning that all eigenvalues ​​of the matrices are greater than zero. Positive definiteness guarantees that the stiffness and damping parameters are positive, avoiding unphysical situations such as negative stiffness or negative damping, and ensuring the mechanical rationality of the impedance control system. The passivity condition is an important constraint to ensure the stability of the impedance control system during interaction with the environment. In this application, the passivity of the system is characterized by the non-negativity of the energy tank state.

[0028] A future time window refers to the length of time that is predicted from the current moment forward, denoted as . .

[0029] A multi-armed intelligent robot refers to a robot system consisting of a mobile platform and at least two robotic arms mounted on the mobile platform. The mobile platform is the mobile body of the multi-armed intelligent robot; it can be a wheeled chassis or a tracked chassis, used to enable the robot's wide-range movement and provide a mounting base for the robotic arms. Each robotic arm is used to perform specific operational tasks. In this application, the body stability refers to the anti-tipping stability of the mobile platform during operation, which is quantified by monitoring the position of the zero-moment point of the mobile platform relative to its supporting polygon. When the movement of the robotic arms generates reaction forces, it may cause a ZMP (Zero Moment Point) shift in the mobile platform, thereby affecting the overall stability of the body.

[0030] Limit Reachability Set Lookup Table: The limit reachability set lookup table is a data table pre-calculated offline and stored in the controller. It uses the sampling points of the robot arm's joint angles and angular velocities as indexes and the coordinates of the convex hull vertices corresponding to each sampling point as values. During online operation, the limit reaction force boundary under the current motion state is quickly obtained by looking up the table.

[0031] Zero Moment Point (ZMP): ZMP is a classic theory for determining the stability of a mobile platform. Physically, it refers to the point of application of the resultant force of the forces exerted by the ground on the mobile platform. When the ZMP is located inside the supporting polygon of the mobile platform, the platform is in a stable state; when the ZMP extends beyond the boundary of the supporting polygon, the platform is at risk of overturning.

[0032] Support polygon: The support polygon is a convex polygonal region defined by the ground contact profile of the wheeled or tracked chassis of the mobile platform, and its boundary constitutes the critical line of the ZMP stability region. The support polygon serves as a spatial reference for calculating stability indices.

[0033] Stability margin: In this application, stability margin is defined as the minimum distance from the ZMP position to the boundary of the supporting polygon. A larger stability margin indicates a more stable mobile platform; a smaller stability margin indicates a more precarious stability and a higher risk of overturning.

[0034] Impedance parameters: Impedance parameters include the stiffness matrix and damping matrix, which describe the dynamic relationship between the force at the end effector of the robotic arm and the position / velocity deviation. By adjusting the impedance parameters, the compliance characteristics of the robotic arm in physical interactions can be changed.

[0035] Stiffness scaling factor: The stiffness scaling factor is a scaling factor between 0 and 1 calculated based on the stability margin in this application. It is used to dynamically adjust the upper bound constraint of the impedance parameter. When the stability margin is sufficient, the scaling factor approaches 1; when the stability margin is tight, the scaling factor approaches 0.

[0036] After introducing the technical terms used in this application, the application scenarios and design concepts of the embodiments of this application will be briefly described below.

[0037] Existing impedance parameter tuning methods ignore the impact of the robotic arm's reaction force on chassis stability, and stability assurance lacks real-time prediction capabilities. Impedance optimization and stability control are disconnected from each other, making it difficult to balance force control accuracy and operational safety.

[0038] Based on this, this application provides a method and system for controlling the stability of a multi-armed intelligent robot.

[0039] Example 1 This application provides a method for controlling the stability of a multi-armed intelligent robot, the multi-armed intelligent robot including a mobile platform and at least two robotic arms mounted on the mobile platform. Figure 1 As shown, the method includes: S110, obtain the current motion state information of the robotic arm.

[0040] S120, based on the current motion state information, predicts the ultimate reaction force information of the robotic arm on the mobile platform within a future time window.

[0041] S130, based on the ultimate reaction force information, calculate the stability index characterizing the current stability of the mobile platform.

[0042] S140, based on the stability index, determine the constraint boundary of the robot arm's impedance parameters. The constraint boundary shrinks as the stability index decreases.

[0043] S150 is optimized by taking the desired motion trajectory and desired contact force as control objectives and the constraint boundary of impedance parameters as optimization constraints, and the optimal impedance parameters are obtained.

[0044] S160 controls the robotic arm to perform motion output based on the optimal impedance parameters.

[0045] The multi-armed intelligent robot stability control method provided in this application achieves proactive perception of chassis stability trends by predicting the ultimate reaction force generated by the robotic arm on the mobile platform within a future time window and calculating stability indices. Based on this, the constraint boundaries of impedance parameters are dynamically contracted according to the stability indices, allowing the compliance characteristics of the robotic arm to adaptively adjust with changes in stability margin, thus balancing high-precision force control with operational safety. When stability is sufficient, the constraint boundaries are relaxed to ensure force tracking accuracy; when stability becomes tight, the boundaries are actively contracted to reduce the impact of reaction forces on the chassis, thereby achieving a dynamic balance between safety and operational performance without additional hardware overhead. Furthermore, by jointly predicting and superimposing the reaction forces of multiple arms, the adverse effects of coupled multi-arm motion on chassis stability are reduced, making it suitable for multi-arm collaborative operation scenarios.

[0046] Step S110 is used to acquire the real-time motion state information of each robotic arm of the multi-armed intelligent robot within the current control cycle, providing a data basis for subsequent prediction of the ultimate reaction force. The acquired current motion state information includes the joint angle vector and joint angular velocity vector of each joint of each robotic arm.

[0047] In practical applications, joint angle vectors and joint angular velocity vectors are obtained in real time through angle sensors installed at each joint. Joint angular velocity information can be obtained either directly by velocity sensors installed at each joint, or by numerically differentiating the joint angle signals collected by the angle sensors. The angle sensors are preferably multi-turn absolute magnetic encoders or optical incremental encoders, which can directly read the absolute position of each joint after the robot system is powered on without requiring a homing operation. In implementations that obtain angular velocity through numerical difference, a tracking differentiator or Kalman filter is used to filter the angle signal to suppress measurement noise introduced by the differential operation.

[0048] The end-effector position and velocity information are obtained through forward kinematics of the robot. Specifically, the end-effector position in Cartesian space is calculated based on the joint angle vectors and the link geometry parameters of the robotic arm, and the joint angular velocities are mapped to the end-effector velocity using the Jacobian matrix. The motion state information of each robotic arm is collected synchronously within the same control cycle to ensure the spatiotemporal consistency of the calculation of the superposition of multi-arm reaction forces.

[0049] In conjunction with the first aspect, step S120 includes: S121 uses the joint angle vector and joint angular velocity vector as a joint query index.

[0050] S122, use the joint query index to search the limit reachable set lookup table to obtain the limit reaction force boundary corresponding to the current motion state.

[0051] The limit reachability lookup table uses the sampling points of joint angle and angular velocity as indexes and the coordinates of the convex hull of the limit reaction force corresponding to each sampling point as values; the limit reaction force boundary is the maximum range of the reaction force that each robotic arm end effector can generate on the mobile platform within a future time window.

[0052] Step S120 utilizes an offline pre-built limit reachability set lookup table to quickly query and obtain the limit reaction force boundary that each robotic arm may generate on the mobile platform within a future time window based on the real-time motion status of each robotic arm. This transforms stability assurance from post-event detection to pre-event prediction, providing a data foundation for the subsequent calculation of stability indicators.

[0053] This limit reachability lookup table is pre-built for multi-armed robotic robots during the offline phase. The construction method involves jointly discretizing and sampling the joint angles and angular velocities of each robotic arm, forming a joint sampling point set composed of combined vectors of joint angles and angular velocities. The preferred number of sampling points is 10. 4 Up to 10 5 For each sampling point in the joint sampling point set, the allowable boundaries of joint torque, joint angle, and joint angular velocity are used as constraints, with a future time window [0, Δ]. t The optimization objective is to maximize the amplitude of the total reaction force generated by the end effector of multiple robotic arms on the moving platform. This involves solving the optimal control problem to obtain the limiting convex hull range of the total reaction force at the sampling point in three-dimensional space. In engineering implementation, a convex hull approximation algorithm is used to store the reachable set as the vertex coordinates of a polyhedron, and then extract the vertex coordinates of the convex hull range. , by joint angle and angular velocity Combined vectors As an index, the vertex coordinates are stored as a limit reachability lookup table. Thus, each index entry in the lookup table corresponds to a specific robotic arm configuration and motion speed state, and its value is the limit boundary that the end effector can reach within a future time window under that state.

[0054] During online execution, S121 is executed first: the current joint angle vectors of each robotic arm collected in step S110 are processed. and joint angular velocity vector This is combined into a joint query index. The joint query index uses... The form completely represents the current motion state of each robotic arm, which is a prerequisite for subsequent query operations. Since different robotic arms may have different joint space dimensions, for those with… The first degree of freedom A robotic arm, whose joint query index vector dimension is... , respectively corresponding Joint angles and Joint angular velocity.

[0055] Then, S122 is executed: the joint query index formed in S121 is searched in the offline pre-built limit reachability set lookup table to obtain the limit reaction force boundary corresponding to the current motion state. Since the offline sampling points are discrete while the actual joint states change continuously, when the joint query index does not find a discrete sampling point, nearest neighbor search or linear interpolation is used to obtain the limit reaction force boundary corresponding to the current state. That is, the coordinates of the convex hull vertex corresponding to the sampling point closest to the current motion state are extracted from the lookup table and used as the limit boundary of the reaction force in the current state. Limit Reaction Force Boundary Expressing the first The maximum range of reaction forces that the end effector of a robotic arm can generate on a mobile platform within a future time window can be specifically represented as the set of coordinates of the convex hull vertices in three-dimensional space { },in, The convex hull contains the number of vertices, covering all possible reaction force directions and amplitudes under this motion state, ensuring that the most unfavorable case is used in the stability assessment. For the multi-arm system, the above table lookup operation is performed on each robotic arm to obtain the limit reaction force boundary for each arm. }, and then pass it to step S130 for superposition of multi-arm reaction forces.

[0056] In conjunction with the first aspect, the limit reachable set lookup table is pre-built offline in the following way: S1200 performs joint discretization sampling of the joint angles and joint angular velocities of each robotic arm of the multi-armed intelligent robot to form a joint sampling point set.

[0057] S1201, for each sampling point in the joint sampling point set, with the joint torque allowable boundary, joint angle allowable boundary and joint angular velocity allowable boundary as constraints, and with the optimization objective of maximizing the amplitude of the total reaction force generated by multiple robotic arm ends on the mobile platform within the future time window, solve the optimal control problem to obtain the limit convex hull range of the total reaction force corresponding to the sampling point in three-dimensional space.

[0058] S1202: Extract the vertex coordinates of the convex hull range, and store the vertex coordinates as a limit reachable set lookup table using the combined vector of joint angles and angular velocities as an index.

[0059] Specifically, step S1200 is used to establish a discrete sampling space covering the possible motion states of each joint of the robotic arm. Specifically, for each joint of the robotic arm, within its physically permissible joint angle range... Within a preset sampling step size, uniform discretization is performed to obtain a discrete value sequence of each joint angle; similarly, within the physically permissible angular velocity range of each joint... The internal sampling is uniformly discretized according to a preset sampling step size to obtain a discrete value sequence of angular velocities for each joint.

[0060] Then, the angles of all joints Discrete values ​​and angular velocities Discrete values ​​are arranged in a full combination to form a joint sampling point set composed of combined vectors of joint angles and angular velocities. For those with The first degree of freedom One robotic arm, each sampling point is a 2 A combined vector of dimensions fully represents the joint positions and motion velocities of the robotic arm in a specific configuration. The number of sampling points can be set according to actual accuracy requirements, preferably 10. 4 Up to 10 5 This allows for control over the storage size of the lookup table while ensuring prediction accuracy.

[0061] The steps are used to calculate the limit reaction force boundary corresponding to each discrete sampling point. For each sampling point in the joint sampling point set, a limit defect analysis problem is constructed. This problem takes the joint configuration corresponding to the sampling point to be analyzed as the starting state, and uses the joint torque allowable boundary, joint angle allowable boundary, and joint angular velocity allowable boundary as constraints. Within the range of these physical constraints, it seeks the motion trajectory that maximizes the amplitude of the total reaction force generated by the multiple robotic arm ends on the moving platform within a future time window. The principle is that, given the initial configuration and velocity, all possible joint motion modes are considered, and the situation with the most severe impact on chassis stability is identified. By solving the above optimal control problem, the range of all possible total reaction forces in three-dimensional space can be obtained for this sampling point. This range is usually presented as a convex hull. This convex hull covers all possible directions and amplitudes of the total reaction force generated in this motion state, ensuring that the most unfavorable situation is used in the stability assessment. By iterating through all sampling points and solving the above problem separately, the limit convex hull range of the total reaction force in three-dimensional space corresponding to each sampling point can be obtained.

[0062] Step S1202 organizes the calculation results of S1201 into a data structure that facilitates rapid online querying. Specifically, for each sampling point, its corresponding limiting convex hull range is a polyhedron in three-dimensional space. This polyhedron covers the direction and magnitude of all possible total reaction forces generated at that sampling point, and its boundary is enclosed by the coordinates of several vertices. In engineering implementation, a convex hull approximation algorithm is used to store the reachable set as the vertex coordinates of the polyhedron. After extracting the convex hull vertex coordinates of each sampling point, the results are then processed using joint angles. and angular velocity The combined vector is used as an index, and the corresponding convex hull vertex coordinates are used as values, stored as a limit reachability set lookup table. This lookup table is organized in an index-value correspondence manner, where each index entry corresponds to a specific robotic arm configuration and motion velocity state, and its value is the limit boundary that the end effector can reach within a future time window under that state.

[0063] After offline pre-calculation is completed, the lookup table is loaded into the controller for quick online lookup. This simplifies the complex optimal control problem that originally needed to be solved online in each control cycle into a table lookup operation, thereby compressing the prediction time for the stability margin of the mobile platform to the millisecond level.

[0064] In conjunction with the first aspect, step S130 includes: S131, Determine the supporting polygon based on the grounding profile of the mobile platform.

[0065] S132, based on the limit reaction force boundary of each robotic arm, calculate the zero-moment point offset caused by the reaction force of each robotic arm according to the zero-moment point theory.

[0066] S133: The zero-torque point offsets of each robotic arm are superimposed and combined with the zero-torque point position of the mobile platform itself to obtain the comprehensive zero-torque point position under extreme conditions.

[0067] S134, calculate the minimum distance from the location of the comprehensive zero moment point to the boundary of the supporting polygon, and use the minimum distance as a stability index.

[0068] Step S130 transforms the limit reaction force boundaries of each robotic arm obtained in step S120 into quantitative indicators characterizing the current stability of the mobile platform, which serve as the basis for subsequent adaptive adjustment of impedance parameters.

[0069] Step S131 is used to determine the stability criterion for the mobile platform. Specifically, the support polygon is determined based on the ground contact profile of the wheeled or tracked chassis of the mobile platform. The vertices of the supporting polygon are formed by the position coordinates of each grounding point of the chassis, and its boundary... The enclosed area is Stable region.

[0070] When ZMP is located inside the supporting polygon, the moving platform is in a stable state; when The moving platform risks overturning if it goes beyond the boundaries of the supporting polygon. The supporting polygon serves as the spatial reference frame for all subsequent stability margin calculations.

[0071] Step S132 is used to quantify the impact of the reaction forces of each robotic arm on the stability of the mobile platform. Based on the limit reaction force boundaries of each robotic arm obtained in step S120, and based on... Theoretical calculations of the reaction forces caused by each robotic arm Offset. Theoretical analysis is a classic method for determining the stability of a mobile platform. Its basic physical meaning is the point of application of the resultant force of the forces exerted by the ground on the mobile platform. When the robotic arm applies a reaction force to the mobile platform, this external force changes the distribution of the ground reaction force, thereby... The position has shifted. For the first... A robotic arm, based on its limit reaction force boundary... The reaction force caused by acting alone can be calculated using static equilibrium equations or dynamic models. offset Each robotic arm The offset reflects the individual impact of each arm on chassis stability under the current motion state, providing basic data for subsequent multi-arm comprehensive effect analysis.

[0072] Step S133 is used to comprehensively evaluate the combined effect of the coupled motion of multiple robotic arms on the stability of the chassis. In a multi-arm system, the reaction forces of each robotic arm will simultaneously affect the stability of the mobile platform. This has an impact, and the effects of each arm are superimposed. Specifically, the data obtained by S132 for each robotic arm... offset By performing vector superposition, the combined effect of the multi-arm reaction force is obtained. Offset. Then synthesize the data. Offset and the effect of the mobile platform's own gravity Location Synthesis yields the synthesis in the limiting case. Location This synthesis process demonstrates the superimposed effect of the coupled motion of multiple robotic arms on chassis stability, and is a key processing step in multi-arm collaborative operation scenarios. This reflects the mobile platform under the most unfavorable conditions (when all robotic arms simultaneously generate ultimate reaction forces). The furthest possible offset position.

[0073] Step S134 is used to synthesize Position is transformed into a quantitative indicator that intuitively reflects the degree of stability. Specifically, the comprehensive value obtained under the extreme case of S133 is calculated. Location To the supporting polygon boundary defined in S131 The minimum distance is defined as the stability index. ,Right now:

[0074] Furthermore, the ultimate reaction force boundary predicted in step S120 is substituted into the above... Offset calculations can yield the predicted limit cases. Location Thus, the predicted stability margin is obtained. The formula for its calculation is:

[0075] in, To support the boundary of the polygon. The physical meaning of taking the minimum distance in the above calculation formula is: for a supporting polygon boundary formed by multiple line segments, it is necessary to calculate separately... The minimum distance from a point to each boundary line segment is taken as the stability margin; this minimum value represents... The direction from which a point is closest to the boundary of the supporting polygon is the direction in which the moving platform is most prone to instability, representing the quantification of the most unfavorable scenario in stability assessment. This stability index... The larger the value, the better the overall performance. The farther the location is from the boundary of the supporting polygon, the more stable the moving platform. The smaller the value, the better the overall stability margin. The closer the location is to the boundary of the supporting polygon, the tighter the stability margin and the higher the risk of overturning.

[0076] when hour, Located inside the supporting polygon, the moving platform is stable; when hour, Located on the boundary, it is in a critically stable state; when hour, If the platform moves beyond the supporting polygon, it becomes unstable. This stability metric... The stability of the mobile platform was quantified using a clear geometric distance, providing an intuitive and comparable benchmark for subsequent differential adjustment of impedance parameters. The calculated stability index... It is passed to step S140.

[0077] In conjunction with the first aspect, step S140 includes: S141, Based on the comparison between the stability index and the preset threshold, determine the current region and its stiffness scaling factor.

[0078] S142, multiply the nominal stiffness upper limit matrix by the stiffness scaling factor to obtain the stiffness upper limit matrix for the current control cycle; and multiply the nominal damping upper limit matrix by the stiffness scaling factor to obtain the damping upper limit matrix for the current control cycle.

[0079] S143 uses the upper limit matrix of stiffness and the upper limit matrix of damping as the constraint boundary of impedance parameters.

[0080] Step S140 is based on the stability index calculated in step S130. (or predict stability margin) The constraint boundary of the robot arm's impedance parameters is dynamically determined. This constraint boundary shrinks as the stability index decreases, thereby achieving adaptive adjustment to ensure accuracy when the stability margin is sufficient and to ensure safety when the stability margin is tight.

[0081] Specifically, the system pre-stores a one-to-one correspondence between each region and each stiffness scaling factor. In step S141, based on the stability index... Or predict stability margin The numerical level of the value determines the region where the current operating state is located, and the stiffness scaling factor is calculated accordingly. .

[0082] Subsequently, step S142 uses the stiffness scaling factor calculated in step S141. This is mapped to specific impedance parameter boundary values. Specifically, the nominal stiffness upper limit matrix is... (i.e., the physical maximum allowable stiffness value) and stiffness scaling factor Multiplying these matrices yields the upper limit stiffness matrix for the current control cycle. ; the nominal damping upper limit matrix (i.e., the physical maximum allowable damping value) and stiffness scaling factor Multiplying these matrices yields the upper damping matrix for the current control cycle. .because ∈[0,1], the above multiplicative scaling ensures that the upper limit matrix of stiffness and the upper limit matrix of damping are always within the physically permissible range [0,1]. ] and [0, The constant variation within the range will not exceed the physical limits of the hardware. As a result, the upper limit matrix of stiffness and the upper limit matrix of damping shrink synchronously as the stability index decreases, realizing the dynamic adjustment of the impedance parameter constraint boundary with the stability margin. Moreover, this adjustment process is continuous and smooth, avoiding system jitter caused by abrupt boundary changes.

[0083] Step S143 is used to convert the stiffness upper limit matrix determined in S142. and damping upper limit matrix It was formally established as the constraint boundary for subsequent optimization problems.

[0084] Specifically, in the quadratic programming optimization problem of step S150, the impedance parameter and Must meet and The upper bound constraint. Through this constraint boundary, when the mobile platform has a sufficient stability margin ( =1), the impedance parameter can be optimized across the entire physical range, prioritizing force tracking accuracy; when the stability margin decreases ( ∈(0,1)), the feasible region of the impedance parameter is dynamically compressed, limiting the equivalent stiffness and damping level of the robotic arm, thereby actively suppressing the transmission strength of the reaction force to the chassis; when the stability margin is severely insufficient ( =0), the impedance parameter is forced to the minimum level to minimize the disturbance of the robot arm movement to the chassis.

[0085] Through the progressive processing of steps S141 to S143, a complete mapping from stability index to impedance parameter numerical constraints is achieved. The stability prediction results are directly incorporated into the online optimization problem of impedance parameters in the form of constraint boundaries, realizing the organic unity of stability control and impedance adaptation.

[0086] In conjunction with the first aspect, step S141 includes: S1411 If the stability index is greater than the first threshold, it is determined that it is in the ample zone, and the stiffness scaling factor is set to the first value.

[0087] S1412 If the stability index is greater than the first threshold and less than or equal to the second threshold, it is determined that the area is in the warning zone. The stiffness scaling factor is calculated linearly according to the relative position of the stability index between the first threshold and the second threshold. The stiffness scaling factor increases as the stability index increases.

[0088] S1413, if the stability index is less than or equal to the first threshold, it is determined to be in the danger zone, and the stiffness scaling factor is set to the second value, which is less than the first value.

[0089] Combination Figure 4 As shown, the horizontal axis represents the predicted stability margin. The values ​​increase from left to right, representing a change in the stability of the mobile platform from low to high. Step S141 is used to determine the stability index. (or predict stability margin) The numerical level of the value determines the region where the current operating state is located, and the stiffness scaling factor is calculated accordingly. Specifically, a first threshold is preset. Second threshold Two key thresholds divide the operating state into three regions. The judgment logic and stiffness scaling factor determination method for each region are as follows: when > At this point, it is determined to be in the ample range. At this time, the mobile platform has sufficient stability margin. The location is a significant safety distance from the boundary of the supporting polygon, posing no risk of overturning. Within this area, to prioritize force tracking accuracy and operational performance, the stiffness scaling factor is adjusted. Set to a first value, preferably 1. When When the value is 1, the stiffness optimization range is fully opened up to the physical maximum value. The controller can pursue the optimal operating performance under this ample stability margin, that is, achieve high-precision tracking of the desired contact force.

[0090] when < ≤ At this point, it is designated as a warning zone. The stability margin of the mobile platform is at a critical state. The location is close to the boundary of the supporting polygon, and the operational intensity of the robotic arm needs to be limited to prevent further deterioration of stability. Within this area, the stiffness scaling factor is calculated linearly according to the relative position of the stability index between the first and second thresholds, i.e.:

[0091] In this formula, when the stability index Approaching the first threshold hour, Approaching 0, the upper limit of stiffness is significantly compressed; when Approaching the second threshold hour, Approaching 1, the upper limit of stiffness is almost completely lifted. Stiffness scaling factor. With stability index The stiffness increases linearly with the increase of the stability margin, ensuring a smooth transition of the upper limit of stiffness with changes in stability margin. Simultaneously, a smoothness constraint on stiffness changes is added within this region to avoid system jitter caused by abrupt stiffness changes and to ensure that the variation of impedance parameters between adjacent control cycles is limited.

[0092] Figure 3 The lower section further specifies the upper limit of stiffness. With forecast stability margin The mathematical expression for a changing S-shaped curve is:

[0093]

[0094] in, The upper limit of stiffness is determined by the predicted stability margin. Changing dynamic values; The nominal maximum stiffness (physical upper limit), i.e., when Asymptotic value at →+∞; The S-shaped adjustment function will predict the stability margin. Mapped to the [0,1] interval; The sensitivity coefficient for the S-curve controls the slope of the curve. The larger the curve is The steeper the change in the vicinity; The midpoint threshold of the S-curve, i.e., σ( The prediction stability margin corresponding to )=0.5. The S-shaped curve at the first threshold. With the second threshold The stiffness boundary was continuously and smoothly adjusted with the stability margin, avoiding sudden jitter during partition switching and making the change of impedance parameters more stable.

[0095] Combination Figure 5 As shown, after dividing the operating state into three regions based on comparison relationships, differentiated control strategies are applied to each region. Specifically: when ≤ At this point, it was determined to be a danger zone. The stability margin was severely insufficient. The location is approaching, and may even exceed, the boundary of the supporting polygon, posing a risk of overturning. Within this region, the stiffness scaling factor should be adjusted. Set to a second value, which is less than the first value, preferably 0. When When the stiffness is 0, the upper limit of stiffness is forced to the minimum level to minimize the disturbance transmission of the robotic arm movement to the chassis. At the same time, the chassis deceleration command is triggered to reduce the movement speed of the mobile platform to increase system damping and implement active stability protection.

[0096] Through the aforementioned three-level zoning mechanism, differentiated control strategies are implemented under different stability margin levels: the ample zone ensures accuracy, the warning zone provides a smooth transition, and the danger zone ensures safety, thus balancing operational performance and safety. In the three zones mentioned above... The values ​​of are all within the range of [0,1], which ensures the continuous variation of the upper limit of stiffness.

[0097] As one feasible approach, the aforementioned first threshold Second threshold Fixed values ​​can be pre-calibrated.

[0098] As another feasible approach, the aforementioned first threshold. Second threshold It can also adaptively and dynamically adjust based on operating conditions such as the current intensity of the robotic arm's movement, load size, arm extension degree, and task requirements, to further improve the environmental adaptability of the control method. Specifically, combined with... Figure 7 As shown, the dynamic threshold determination process includes three steps: Step 1: Calculate the overall safety factor.

[0099] Considering multiple factors affecting the stability of the mobile platform, including: the intensity of movement of the robotic arm joints (i.e., the amplitude of velocity and acceleration of each joint and the end effector velocity; more intense movement results in greater impact on the chassis and a higher safety factor); load size (i.e., the weight of the workpiece carried or operated by the robotic arm end effector; a larger load results in a greater reaction force and a higher safety factor); arm extension (i.e., the extension distance of the robotic arm end effector relative to the base; a longer extension results in a larger lever arm of the reaction force on the chassis and a higher safety factor); and task requirements (i.e., the force control accuracy or motion accuracy required for the current operation; higher accuracy requirements or lower task urgency result in a higher safety factor). The values ​​of all the above factors are normalized to the [0,1] interval. After obtaining the normalized values ​​of the above factors, a comprehensive safety factor is calculated. It can be calculated using a weighted summation method, that is, summing the results after assigning different weight coefficients to each factor; or it can be determined by taking the maximum value among all factors. It equals the maximum value among all factors. The advantage of the weighted summation method is that the contribution of each factor can be finely adjusted, making it suitable for scenarios where factors have complementary relationships; the advantage of the maximum value method is that the threshold is raised when any factor deteriorates, making it suitable for scenarios where safety is paramount. The two methods can be selected according to specific application requirements. Through the above methods, Mapped to the [0,1] interval, the more dangerous the working condition (the more intense the movement, the greater the load, the longer the stretch, the higher the task requirements). The closer to 1, the safer the operating condition. The closer it is to 0.

[0100] The second step is to calculate the adaptive threshold.

[0101] In obtaining the comprehensive safety factor After that, the threshold followed Dynamic adjustment, the more dangerous the working conditions ( The larger the threshold, the more stringent the zoning; the safer the operating conditions. The smaller the value, the lower the threshold, and the more lenient the partitioning. The specific calculation method is as follows:

[0102]

[0103] in, The nominal first threshold represents the minimum stability margin. The first threshold sensitivity coefficient is used to control... right The extent of the impact; The nominal interval width is the nominal distance between the ample zone and the danger zone. The interval width sensitivity coefficient controls... The degree of influence on the interval width.

[0104] Combining the above formula, when the working condition becomes dangerous ( When (increases), and Synchronous increases expand the danger zone and shrink the safety margin, achieving adaptive contraction of the safety margin; when the operating condition becomes safer ( (when decreasing) and Synchronous lowering reduces the danger zone and expands the ample zone, allowing the robotic arm to perform its operations over a wider range.

[0105] Step 3: Dynamic Partitioning Based on the adaptive threshold calculated in the second step and Following the same judgment logic as described above, the current operating state is divided into an adequate zone, a warning zone, or a danger zone. Compared with a fixed threshold, a dynamic threshold allows the zone boundaries to move in real time according to the working conditions. When the robotic arm moves rapidly, carries a heavy load, or extends significantly, the threshold automatically increases, and the system enters the warning zone or danger zone earlier, triggering a more conservative impedance parameter adjustment. When the robotic arm moves slowly, is unloaded, or is in a retracted configuration, the threshold automatically decreases, and the system enters the warning zone or danger zone later, allowing for a more aggressive impedance parameter adjustment.

[0106] like Figure 5 As shown in the figure, the comprehensive safety factor is illustrated exemplarily. When taking different values, the first threshold and The change in position, that is The larger the threshold, the higher the first and second thresholds rise simultaneously, the smaller the area of ​​the sufficiency zone and the larger the area of ​​the danger zone; conversely... The smaller the threshold, the lower the first and second thresholds decrease simultaneously, expanding the margin area while shrinking the danger zone. Through the aforementioned adaptive dynamic threshold mechanism, this invention enables the zoning criteria for stable margins to be adjusted in real time according to changes in operating conditions, avoiding the problems of fixed thresholds being too aggressive under extreme conditions and too conservative under safe conditions, further improving the adaptability and robustness of the control method in dynamic and uncertain environments.

[0107] In conjunction with the first aspect, step S150 includes: S151, obtain the desired motion trajectory and desired contact force. The desired motion trajectory includes the desired end position and desired end velocity.

[0108] S152 constructs a cost function for a quadratic programming optimization problem with the goal of minimizing the weighted sum of squares of force tracking error, motion trajectory tracking error, and the deviation of impedance parameters from their nominal values.

[0109] S153 uses the upper bound matrix of stiffness and the upper bound matrix of damping as the upper bound constraints of impedance parameters, the physically permissible minimum stiffness and minimum damping as the lower bound constraints of impedance parameters, and the positive definiteness condition and the passivity condition of the system as the feasibility constraints to construct the constraints of the quadratic programming optimization problem.

[0110] S154 solves for the stiffness and damping matrices that minimize the cost function under constraints, and outputs them as the optimal impedance parameters.

[0111] Step S150 uses the upper limit stiffness matrix and upper limit damping matrix determined in step S140 as constraint boundaries, and the desired motion trajectory and desired contact force as control objectives. By solving the quadratic programming (QP) optimization problem, the optimal impedance stiffness matrix and damping matrix are obtained, thereby achieving multi-objective coordinated optimization between force tracking accuracy, trajectory tracking accuracy and system stability.

[0112] Step S151 is used to obtain the desired control objective required for subsequent optimization. The desired motion trajectory and desired contact force are obtained as follows: In the offline phase, the operator performs one or more operation demonstrations through an admittance-type physical interface (such as gravity compensation or force control mode). During this process, the end-effector position trajectory and contact force data are recorded in real time to form a demonstration dataset. After the demonstration is completed, the collected multiple sets of demonstration data are encoded using a Gaussian Mixture Model (GMM) to learn the probability distribution model of the operation trajectory and contact force. During online operation, the above model is regressed using Gaussian Mixture Regression (GMR) to generalize and generate the desired position trajectory and desired contact force. Among them, the desired position trajectory includes the desired end-effector position. and expected terminal velocity The expected contact force is denoted as The aforementioned method for generating desired trajectories based on imitation learning enables robots to automatically acquire reference trajectories and forces that meet process requirements through operational demonstrations, avoiding the tedious process of manual programming and teaching for different tasks required in traditional methods.

[0113] Step S152 is used to quantify multiple control objectives into a unified optimization index and construct the objective function of the QP problem. Specifically, the optimization objectives of the quadratic programming optimization problem are: to minimize the force tracking error, that is, to minimize the deviation between the actual contact force and the desired contact force, so as to ensure force control accuracy; to minimize the motion trajectory tracking error, that is, to minimize the deviation between the actual end position and the desired end position, so as to ensure trajectory tracking accuracy; and to minimize the degree of deviation of the impedance parameters from the nominal value, that is, to suppress excessive fluctuations of the stiffness matrix and damping matrix relative to the nominal value, so as to ensure control smoothness and continuity of parameter changes.

[0114] The three objectives described above reflect the performance requirements in terms of force control accuracy, motion accuracy, and parameter smoothness, respectively. They are transformed into a single scalar cost function through weighted summation. The weighting matrices are used to adjust the relative importance of each optimization objective and can be flexibly adjusted according to different operational needs. For example, in grinding scenarios with high force control accuracy requirements, the weight of the force tracking error term can be increased; in material handling scenarios with high trajectory tracking requirements, the weight of the trajectory tracking error term can be increased. This cost function transforms the original multi-objective optimization problem into a single-objective QP problem that is easy to solve in real time.

[0115] Step S153 formally establishes the impedance parameter boundary determined in step S140 as the mathematical constraint for the QP optimization problem. Specifically, the constraint conditions include the following three aspects: 1) Upper Bound Constraints: The upper bound matrix of stiffness and the upper bound matrix of damping determined in step S142 serve as the upper bounds of the impedance parameters. That is, the stiffness matrix and damping matrix of the optimized output do not exceed the corresponding upper bound matrices. This constraint ensures that the impedance parameters of the optimized output will not exceed the maximum value allowed by the current stability margin, thus directly embedding the stability prediction results into the optimization process in the form of constraint boundaries. In particular, this predicted stability margin constraint directly links the extreme case of the robotic arm's reaction force with the chassis support capacity, which is one of the core features that distinguishes this invention from the prior art.

[0116] (2) Lower bound constraint: The physical minimum stiffness and minimum damping are used as the lower bound of the impedance parameters, that is, the stiffness matrix and damping matrix of the optimized output are not lower than the corresponding physical lower bounds. This constraint ensures that the impedance parameters will not be lower than the lower bound of the hardware physical implementation, ensuring the physical implementability of the control commands.

[0117] (3) Feasibility Constraints: The positive definiteness condition and the passive nature condition together constitute the feasibility constraints. Among them, the positive definiteness condition ensures the physical rationality of the stiffness matrix and damping matrix, that is, both stiffness and damping are positive values, avoiding non-physical situations such as negative stiffness or negative damping; the passive nature condition (characterized by the non-negative state of the energy tank) ensures that the impedance control system will not actively inject energy into the system when interacting with the environment, thereby ensuring the stability and safety of the interaction process. Based on the above constraints, the feasible domain of the impedance parameters is fully defined.

[0118] Step S154 is used to solve the above QP optimization problem and output the optimization results. Specifically, in the quadratic programming optimization problem formed by the cost function constructed in S152 and the constraints constructed in S153, the stiffness matrix and damping matrix that minimize the cost function are solved. Since the objective function of this optimization problem is quadratic and the constraints are linear matrix inequalities, it belongs to a standard convex quadratic programming problem and has a global optimal solution. It can be solved in milliseconds using mature algorithms such as the effective set method and the interior point method, meeting the real-time operation requirements of the upper optimization layer. The optimal stiffness matrix and optimal damping matrix obtained are output as optimal impedance parameters to step S160. This optimization solution process is repeated in each control cycle, so that the impedance parameters can be continuously and adaptively updated as the stability margin of the moving platform changes, realizing real-time response to dynamic uncertain environments.

[0119] In conjunction with the first aspect, step S160 includes: S161, the upper layer operates at the first preset frequency, and outputs the current optimal impedance parameters based on the current stability index, the current impedance parameter constraint boundary, and the solution of the current optimization problem.

[0120] S162, the middle layer operates at the second preset frequency, and executes the Cartesian space impedance control law according to the current stiffness matrix and the current damping matrix in the current optimal impedance parameters to calculate the expected end force; wherein, the second preset frequency is higher than the first preset frequency.

[0121] S163, the bottom layer operates at the third preset frequency, and calculates the driving torque of each joint motor according to the expected end force through the joint torque control algorithm; wherein, the third preset frequency is higher than the second preset frequency.

[0122] S164, for each robotic arm, drives the robotic arm to perform motion output with the corresponding drive torque.

[0123] Step S160 employs a three-layer hierarchical control architecture, converting the optimal impedance parameters output in step S150 into actual joint drive torques to drive each robotic arm to execute motion outputs, achieving hierarchical coordination of stability prediction, impedance adaptation, and servo control. The three-layer hierarchical control architecture separates control tasks according to different time scales, specifically combining... Figure 6 As shown, it includes: Upper-layer optimization: This layer operates at a first preset frequency and handles computationally intensive tasks. It performs the limit reachability set lookup table prediction in step S120, the stability index calculation in step S130, the impedance parameter constraint boundary determination in step S140, and the quadratic programming optimization solution in step S150. It then outputs the obtained optimal stiffness matrix and optimal damping matrix to the middle layer. This lower frequency provides ample time for the upper-layer optimization calculations, ensuring the real-time feasibility of solving the optimization problem. In this embodiment, the first preset frequency is 100~500Hz.

[0124] Intermediate Impedance Control Layer: Operates at a second preset frequency, performing Cartesian space impedance control. This frequency is higher than the first preset frequency of the upper optimization layer. This layer receives the optimal stiffness matrix and optimal damping matrix output from the upper layer, as well as the desired trajectory (desired end position, desired end velocity) and desired contact force obtained in step S151. It calculates the desired end force using the Cartesian space impedance control law.

[0125] in, and This refers to the actual end position and velocity fed back from the bottom layer. The desired end force output by this layer is transmitted to the bottom layer. In this embodiment, the second preset frequency is 1~4kHz.

[0126] The bottom-level torque control layer operates at a third preset frequency, performing joint servo control. This frequency is higher than the second preset frequency of the middle-level impedance control layer. This layer receives the desired end force output from the middle layer and, combined with feedback signals such as current, position, and velocity, calculates the actual driving torque of each joint motor using a joint torque control algorithm (such as PID control or sliding mode control). This drives the physical actuators (joint motors) to complete the motion output, directly interacting with the robot hardware to achieve the final torque closed loop. In this embodiment, the third preset frequency is 4~8kHz.

[0127] Closed-loop feedback mechanism: The bottom-level torque control layer feeds back the measured position, velocity, and torque information to the middle-level impedance control layer and the upper-level optimization layer. The middle layer uses the feedback values ​​to calculate the impedance control law, and the upper layer uses the feedback values ​​to update the stability prediction and optimization model, forming a complete closed-loop real-time control. The entire online control process is executed cyclically with a fixed period, enabling the robot to maintain stable operational performance in dynamic environments. Through the above three-layer hierarchical architecture, time-scale decoupling of slow optimization, medium-speed coordination, and fast execution is achieved, ensuring a balance between the real-time performance and computational efficiency of the control system.

[0128] Combination Figure 2 , Figure 3As shown, the method is divided into two main parts: an offline stage and an online stage. The offline stage completes the pre-calculation of the limit reachability set lookup table and the training of the desired trajectory model. The online stage executes real-time control in a fixed control cycle, thereby achieving online coordination of stability prediction and impedance adaptation.

[0129] like Figure 2 As shown, in the offline phase, a unified dynamic model of the multi-armed intelligent robot is first established, mathematically describing the mobile platform and at least two robotic arms mounted on it as a whole, providing a physical model foundation for subsequent calculations of ultimate reaction forces and stability analysis. Then, for different joint configurations and motion speeds of each robotic arm, a convex hull approximation algorithm is used to pre-calculate the limit reachability set lookup table offline. Figure 3 As shown in the dashed box on the left, the construction process of this lookup table is as follows: taking the joint space sampling points of the robotic arm (including the joint discretized sampling points of joint angle and joint angular velocity), the dynamic parameters of the robotic arm (mass, inertia, link geometry parameters), and the allowable boundary of joint torque as input, the optimal control problem is solved for each sampling point through the limit reachability set calculation engine to obtain the limit convex hull range of the reaction force / torque that the sampling point may generate in the future time window; then, the coordinates of the convex hull vertex are extracted, and the vertex coordinates are stored as a limit reachability set lookup table with the combined vector of joint angle and angular velocity as index. This lookup table is indexed by (joint angle, angular velocity), and the corresponding value is the convex hull vertex coordinates of the reaction force in the three directions. It is stored in the controller for online querying. Simultaneously, the operator demonstrates single or multiple operations via an admittance-type physical interface, collecting and recording end-effector position trajectory and contact force data to form a demonstration dataset. Based on this, a Gaussian mixture model is used to encode the demonstration data, and Gaussian mixture regression is used to generate the desired position trajectory and desired contact force online. This allows the robot to automatically acquire reference trajectories and forces that meet process requirements through imitation learning. All parameters pre-calculated in the offline phase (including the limit reachability lookup table and the desired trajectory generation model) are loaded and then enter the online phase.

[0130] During the online phase, each step is executed cyclically at a fixed time. First, current sensor data is read, including the joint angles and angular velocities of each robotic arm, end-effector contact forces, and the zero-torque point information of the moving platform. Then, using the current joint angles and angular velocities of each robotic arm as a joint query index, a search is performed in an offline pre-calculated limit reachability set lookup table. The limit reaction force boundary corresponding to the current motion state is obtained through nearest neighbor search or linear interpolation. Based on this limit reaction force boundary, the reaction forces caused by each robotic arm are calculated according to the zero-torque point theory. Offset, which is the sum of the offsets of each arm and the offset of the mobile platform itself. Positional synthesis yields the synthesis in the limiting case. The location is determined, and the minimum distance from that location to the boundary of the supporting polygon is calculated as a stability index. After obtaining the stability index, it is compared with a preset first threshold and a second threshold to determine the current region (sufficient region, warning region, or danger region). A stiffness scaling factor λ is calculated based on the region type. When the stability index is greater than the second threshold, it is in the sufficient region, and λ is set to 1. When the stability index is between the first and second thresholds, it is in the warning region, and λ is calculated using linear interpolation. When the stability index is less than or equal to the first threshold, it is in the danger region, and λ is set to 0. After determining the stiffness scaling factor, the nominal stiffness upper limit matrix is ​​multiplied by λ to obtain the stiffness upper limit matrix for the current control cycle, and the nominal damping upper limit matrix is ​​multiplied by λ to obtain the damping upper limit matrix for the current control cycle, serving as the constraint boundary for the impedance parameters. Subsequently, using the desired trajectory and desired force generated in the offline stage as objectives, and the aforementioned stiffness upper limit matrix and damping upper limit matrix as optimization constraints, a quadratic programming optimization problem is constructed and solved, outputting the optimal impedance stiffness matrix and damping matrix. Finally, hierarchical impedance control is executed. The optimal stiffness and damping matrices obtained from the solution are substituted into the middle-level impedance controller, which runs the Cartesian space impedance control law at a frequency of 1kHz to 4kHz to calculate the desired end force. The bottom-level controller runs joint torque control at a frequency of 4kHz to 8kHz to track the desired end force and complete the final drive. After completing the above steps, the control flow returns to the step of reading sensor data and enters the next control cycle, forming a closed-loop real-time control.

[0131] Through the control process that combines offline pre-calculation with online table lookup optimization, this invention achieves the organic unity of stability prediction and adaptive adjustment of impedance parameters while ensuring real-time performance.

[0132] Secondly, this application provides a stability control system for a multi-armed intelligent robot, the multi-armed intelligent robot comprising a mobile platform and at least two robotic arms disposed on the mobile platform, characterized in that it includes: The stability prediction module is used to acquire the current motion state information of the robotic arm, predict the ultimate reaction force information of the robotic arm on the mobile platform within a future time window based on the current motion state information, and calculate the stability index characterizing the current stability of the mobile platform based on the ultimate reaction force information.

[0133] The impedance parameter optimization module is used to determine the constraint boundary of the robot arm's impedance parameter based on the stability index. The constraint boundary shrinks as the stability index decreases. The module solves the optimization problem with the desired motion trajectory and desired contact force as control objectives and the constraint boundary of the impedance parameter as optimization constraints to obtain the optimal impedance parameter.

[0134] The hierarchical control execution module is used to control the robotic arm to perform motion output based on the optimal impedance parameters.

[0135] In conjunction with the second aspect, the hierarchical control execution module includes: an upper-level optimization layer controller, a middle-level impedance control layer controller, and a lower-level torque control layer controller.

[0136] The upper-level optimization layer controller is used to operate at the first preset frequency, perform optimization solutions based on the current stability index and the current impedance parameter constraint boundary, and output the current optimal impedance parameter. The middle impedance control layer controller is used to operate at a second preset frequency. It executes the Cartesian space impedance control law based on the stiffness matrix and damping matrix in the current optimal impedance parameters and calculates the desired end force. The second preset frequency is higher than the first preset frequency. The bottom-level torque control layer controller is used to operate at a third preset frequency. Based on the desired end force, it calculates the driving torque of each joint motor through the joint torque control algorithm and drives each robotic arm to perform motion output. The third preset frequency is higher than the second preset frequency.

[0137] Thirdly, embodiments of this application provide an electronic device, which includes a memory and a processor. The memory stores a computer program, and the processor runs the computer program to cause the electronic device to perform the above-described method.

[0138] Furthermore, combined Figure 3 The electronic device shown also includes a bus and a communication interface, with the processor, communication interface, and memory connected via the bus.

[0139] The memory may include high-speed random access memory (RAM) and may also include non-volatile memory, such as at least one disk drive. Communication between this system network element and at least one other network element is achieved through at least one communication interface (wired or wireless), which can use the Internet, wide area network, local area network, metropolitan area network, etc. The bus can be an ISA bus, PCI bus, or EISA bus, etc. The bus can be divided into address bus, data bus, control bus, etc.

[0140] The processor may be an integrated circuit chip with signal processing capabilities. In implementation, the steps of the above methods can be completed by integrated logic circuits in the processor's hardware or by software instructions. The processor can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc.; it can also be a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this invention. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in the embodiments of this invention can be directly embodied in the execution of a hardware decoding processor, or executed by a combination of hardware and software modules in the decoding processor. The software modules can reside in random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or other mature storage media in the art. The storage medium is located in the memory, and the processor reads the information in the memory and, in conjunction with its hardware, completes the steps of the method described in the foregoing embodiments.

[0141] Fourthly, embodiments of this application provide a readable storage medium storing computer program instructions, which are read and executed by a processor to perform the above-described method.

[0142] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working process of the system and apparatus described above can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0143] Furthermore, in the description of the embodiments of the present invention, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in the present invention based on the specific circumstances.

[0144] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0145] In the description of this invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing the invention and for simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0146] Finally, it should be noted that the above embodiments are merely specific implementations of the present invention, used to illustrate the technical solutions of the present invention, and not to limit it. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments within the technical scope disclosed in the present invention, or make equivalent substitutions for some of the technical features; and these modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for controlling the stability of a multi-armed intelligent robot, the multi-armed intelligent robot comprising a mobile platform and at least two robotic arms disposed on the mobile platform, characterized in that, The method includes: Obtain the current motion state information of the robotic arm; Based on the current motion state information, predict the ultimate reaction force information of the robotic arm on the mobile platform within a future time window; Based on the ultimate reaction force information, a stability index characterizing the current stability of the mobile platform is calculated; Based on the stability index, the constraint boundary of the impedance parameter of the robotic arm is determined, and the constraint boundary contracts as the stability index decreases. The optimal impedance parameters are obtained by optimizing the solution with the desired motion trajectory and desired contact force as control objectives and the constraint boundary of the impedance parameters as optimization constraints. Based on the optimal impedance parameters, the robotic arm is controlled to perform motion output.

2. The method according to claim 1, characterized in that, The current motion state information includes the current joint angle vector and joint angular velocity vector of the robotic arm; The step of predicting the ultimate reaction force of the robotic arm on the mobile platform within a future time window based on the current motion state information includes: Use the joint angle vector and the joint angular velocity vector as a joint query index; The joint query index is used to search the limit reachability set lookup table to obtain the limit reaction force boundary corresponding to the current motion state; The limit reachable set lookup table uses the sampling points of joint angle and angular velocity as indexes and the coordinates of the convex hull of the limit reaction force corresponding to each sampling point as values; the limit reaction force boundary is the maximum range of the reaction force that each robotic arm end effector can generate on the mobile platform within a future time window.

3. The method according to claim 2, characterized in that, The limit reachable set lookup table is pre-built offline using the following method: The joint angles and joint angular velocities of each robotic arm of the multi-armed intelligent robot are jointly discretized and sampled to form a joint sampling point set; For each sampling point in the joint sampling point set, with the joint torque allowable boundary, joint angle allowable boundary, and joint angular velocity allowable boundary as constraints, and with the optimization objective of maximizing the amplitude of the total reaction force generated by multiple robotic arm ends on the mobile platform within a future time window, the optimal control problem is solved to obtain the limit convex hull range of the total reaction force corresponding to the sampling point in three-dimensional space. Extract the vertex coordinates of the convex hull range, and store the vertex coordinates as a limit reachable set lookup table using the combined vector of joint angles and angular velocities as an index.

4. The method according to claim 1, characterized in that, The step of calculating the stability index characterizing the current stability of the mobile platform based on the ultimate reaction force information includes: The supporting polygon is determined based on the grounding profile of the mobile platform; Based on the limit reaction force boundary of each robotic arm, the offset of the zero moment point caused by the reaction force of each robotic arm is calculated based on the zero moment point theory. The zero-moment point offsets of each robotic arm are superimposed and combined with the zero-moment point position of the mobile platform itself to obtain the comprehensive zero-moment point position under extreme conditions. Calculate the minimum distance from the location of the integrated zero moment point to the boundary of the supporting polygon, and use the minimum distance as the stability index.

5. The method according to claim 1, characterized in that, The step of determining the constraint boundary of the impedance parameters of the robotic arm based on the stability index includes: Based on the comparison between the stability index and the preset threshold, the current region and its stiffness scaling factor are determined. Multiply the nominal stiffness upper limit matrix by the stiffness scaling factor to obtain the stiffness upper limit matrix for the current control cycle; and multiply the nominal damping upper limit matrix by the stiffness scaling factor to obtain the damping upper limit matrix for the current control cycle. The stiffness upper limit matrix and the damping upper limit matrix are used as the constraint boundaries of the impedance parameters.

6. The method according to claim 5, characterized in that, The step of determining the current region and its stiffness scaling factor based on the comparison between the stability index and a preset threshold includes: If the stability index is greater than the first threshold, it is determined that the region is in the sufficiency zone, and the stiffness scaling factor is set to the first value. If the stability index is greater than the first threshold and less than or equal to the second threshold, it is determined that the area is in the warning zone. The stiffness scaling factor is calculated linearly according to the relative position of the stability index between the first threshold and the second threshold. The stiffness scaling factor increases as the stability index increases. If the stability index is less than or equal to the first threshold, it is determined to be in a danger zone, and the stiffness scaling factor is set to a second value, which is less than the first value.

7. The method according to claim 5, characterized in that, The steps for solving the optimization problem to obtain the optimal impedance parameters, using the desired motion trajectory and desired contact force as control objectives and the constraint boundary of the impedance parameters as optimization constraints, include: Obtain the desired motion trajectory and desired contact force, wherein the desired motion trajectory includes the desired end position and desired end velocity; A cost function for a quadratic programming optimization problem is constructed with the goal of minimizing the weighted sum of squares of force tracking error, motion trajectory tracking error, and the deviation of impedance parameters from their nominal values. The constraints of the quadratic programming optimization problem are constructed using the upper bound matrix of stiffness and the upper bound matrix of damping as upper bound constraints of impedance parameters, the physically permissible minimum stiffness and minimum damping as lower bound constraints of impedance parameters, and the positive definiteness condition and the system passivity condition as feasibility constraints. Under the constraints, the stiffness matrix and damping matrix that minimize the cost function are solved and output as the optimal impedance parameters.

8. The method according to claim 7, characterized in that, The step of controlling the robotic arm to perform motion output according to the optimal impedance parameter includes: The upper optimization layer runs at a first preset frequency and outputs the current optimal impedance parameter based on the current stability index, the current impedance parameter constraint boundary, and the solution of the current optimization problem. The middle impedance control layer operates at a second preset frequency and executes the Cartesian space impedance control law based on the current stiffness matrix and the current damping matrix in the current optimal impedance parameters to calculate the desired end force; wherein, the second preset frequency is higher than the first preset frequency; The bottom-level torque control layer operates at a third preset frequency, and calculates the driving torque of each joint motor according to the desired end force through a joint torque control algorithm; wherein, the third preset frequency is higher than the second preset frequency; For each of the robotic arms, the robotic arm is driven to perform motion output with the corresponding drive torque.

9. A stability control system for a multi-armed intelligent robot, the multi-armed intelligent robot comprising a mobile platform and at least two robotic arms disposed on the mobile platform, characterized in that, include: The stability prediction module is used to acquire the current motion state information of the robotic arm, predict the ultimate reaction force information of the robotic arm on the mobile platform in a future time window based on the current motion state information, and calculate the stability index characterizing the current stability of the mobile platform based on the ultimate reaction force information. The impedance parameter optimization module is used to determine the constraint boundary of the impedance parameter of the robotic arm based on the stability index. The constraint boundary shrinks as the stability index decreases. The module solves the optimization problem with the desired motion trajectory and desired contact force as control targets and the constraint boundary of the impedance parameter as optimization constraints to obtain the optimal impedance parameter. The hierarchical control execution module is used to control the robotic arm to perform motion output according to the optimal impedance parameters.

10. The system according to claim 9, characterized in that, The hierarchical control execution module includes: The upper-level optimization layer controller is used to operate at the first preset frequency, perform optimization solutions based on the current stability index and the current impedance parameter constraint boundary, and output the current optimal impedance parameter. The middle impedance control layer controller is used to operate at a second preset frequency, execute the Cartesian space impedance control law according to the stiffness matrix and damping matrix in the current optimal impedance parameters, and calculate the desired end force. The second preset frequency is higher than the first preset frequency. The bottom-level torque control layer controller is used to operate at a third preset frequency. Based on the desired end force, it calculates the driving torque of each joint motor through a joint torque control algorithm and drives each of the robotic arms to perform motion output. The third preset frequency is higher than the second preset frequency.

Images