Method and system for calculating mass center position of robot

Abstract

The invention provides a method and system for calculating the mass center position of a robot. The method comprises the steps that the mass of a mechanical arm connecting rod, the height of the mass center of the mechanical arm connecting rod to the ground, the mass center coordinates of the mechanical arm connecting rod under an original coordinate system, the angular speed of the mechanical arm connecting rod, the actually-measured mechanical arm joint angle, the actually-measured mechanical arm joint angular speed and the actually-measured mechanical arm joint external force are obtained; based on the data, a first mechanical arm kinetic equation and a mechanical arm joint angle error are obtained, and a mechanical arm joint angular velocity error is calculated; if the actually-measured mechanical arm joint external force is smaller than or equal to the preset external force threshold value, position control mode operation is executed, and position control mode torque is obtained; and calculating the mass center position of the robot based on the position control mode torque, the preset rotation matrix, the mechanical arm connecting rod mass center coordinates and the mechanical arm connecting rod mass under the original coordinate system. According to the method for calculating the mass center position of the robot, the overturning risk caused by mass center deviation of the robot can be avoided, and the balance stability of the robot is improved.

Description

Technical Field

[0001] This invention relates to the field of humanoid robot control technology, and in particular to a method and system for calculating the center of mass position of a robot. Background Technology

[0002] When humanoid robots perform tasks in complex environments, maintaining balance at multiple points of contact is a key challenge. Specifically, multi-point balance refers to the robot's ability to maintain balance and complete tasks while climbing, pushing objects, or walking on uneven ground, requiring the simultaneous use of multiple contact points (such as hands, feet, and arms). To achieve this, the robot needs high compliance and precise center of mass control. In the construction of smart grids, humanoid robots are often used for intelligent inspections to detect anomalies in factories. During power grid inspections, humanoid robots often need to cross devices such as rodent barriers. In this case, their robotic arms require compliant control to protect the joints and walls, preventing damage from rigid contact. As one of the most important components of a humanoid robot, the robotic arm has a significant mass, and its positional changes during compliant contact with walls often lead to changes in the robot's center of mass. The position of the center of mass is a crucial indicator of robot stability and is significant in the multi-point balance process of humanoid robots. Lack of effective control over this can lead to the humanoid robot tipping over.

[0003] In the current technological context, research on multi-contact balance control for humanoid robots has not fully considered the key factor of changes in the center of mass caused by changes in the position of the robotic arm. Because the dynamic position of the robotic arm and the changes in the center of mass have not been systematically correlated and targeted control has not been implemented, humanoid robots cannot ensure the stability of the robot's center of mass when dealing with scenarios that require compliant robotic arm movements, such as crossing mouse barriers during power line inspections. This affects the reliability of the overall balance control and fails to meet the high-precision requirements for robot stability in complex task environments. Summary of the Invention

[0004] The present invention aims to provide a method and system for calculating the center of mass position of a robot to solve the above-mentioned technical problems, avoid the risk of tipping over due to the displacement of the center of mass caused by changes in the position of the robotic arm during multi-contact balance control, realize real-time and accurate control of the center of mass position of the humanoid robot, and thus improve the balance stability of the humanoid robot.

[0005] To address the aforementioned technical problems, this invention provides a method for calculating the centroid position of a robot, comprising: Obtain the mass of several robotic arm links, the height of the center of mass of several robotic arm links relative to the ground, the coordinates of the center of mass of several robotic arm links in the original coordinate system, the angular velocities of several robotic arm links, the measured joint angles of the robotic arm, the measured joint angular velocities of the robotic arm, and the measured external forces of the robotic arm joints; Based on the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, the angular velocity of the robotic arm link, the measured robotic arm joint angle, and the measured robotic arm joint angular velocity, the first robotic arm dynamic equation, the robotic arm joint angle error, and the calculated robotic arm joint angular velocity error are obtained. If the measured external force on the robotic arm joint is less than or equal to the preset external force threshold, the position control mode operation is executed to obtain the position control mode torque; Based on the position control mode torque, preset rotation matrix, coordinates of the robot arm link centroid in the original coordinate system and mass of the robot arm link, calculate the position of the robot centroid. The position control mode operation includes: Based on the joint angle error of the robotic arm, the joint angular velocity error of the robotic arm, and the first dynamic equation of the robotic arm, the state feedback compensation and the state observer compensation are determined. The position control mode torque is obtained based on state feedback compensation and state observer compensation.

[0006] In the above scheme, by establishing the first robotic arm dynamics equation, the mathematical relationship between the joint motion and force of the robotic arm is clarified, providing a theoretical model for control compensation calculation. Simultaneously, the joint angle error and joint angular velocity error of the robotic arm are calculated, quantifying the deviation between the actual and ideal states, and providing a basis for error correction in the position control mode. Next, when the measured external force on the robotic arm joint is less than or equal to a preset external force threshold, the position control mode operation is triggered, ensuring that the control mode is adapted to the actual force conditions of the robotic arm. Then, based on the joint angle error, joint angular velocity error, and the first robotic arm dynamics equation, the position control mode torque is obtained. This position control mode torque ensures the motion accuracy and stability of the robotic arm in low-force scenarios. Finally, the spatial posture of the current robotic arm is obtained by updating the position control mode torque in combination with the preset rotation matrix. Based on the updated rotation matrix, the coordinates of the center of mass of the robotic arm link in the original coordinate system are transformed. Then, the position of the robot's center of mass is obtained by combining the mass of the robotic arm link to measure the robot's balance state. This avoids the risk of tipping over caused by the displacement of the center of mass due to changes in the position of the robotic arm during the multi-contact balance control process of the humanoid robot. This achieves real-time and accurate control of the center of mass position of the humanoid robot, thereby improving the balance stability of the humanoid robot.

[0007] Furthermore, it also includes: If the measured external force on the robotic arm joint is greater than the preset external force threshold, the impedance control mode operation is executed to obtain the impedance control mode torque; Based on the impedance control mode torque, the preset rotation matrix, the coordinates of the robot arm link's center of mass in the original coordinate system, and the mass of the robot arm link, the position of the robot's center of mass is calculated. The impedance control mode operation includes: Based on the joint angle error, joint angular velocity error, preset positive definite joint mass matrix, preset positive definite damping matrix, preset positive definite stiffness matrix, and measured external force of the joint, an impedance dynamics equation is established. Based on the impedance dynamics equation and the first robotic arm dynamics equation, the torque of the impedance control mode is obtained.

[0008] In the above scheme, if the measured external force on the robotic arm joint exceeds the preset external force threshold, the impedance control mode operation is triggered. This ensures that the control mode is compatible with the working conditions where the robotic arm is subjected to large external forces, preventing damage to the robotic arm joints or the wall surface due to external force overload. In the impedance control mode operation, an impedance dynamics equation is first established based on the robotic arm joint angle error, the robotic arm joint angular velocity error, and the measured external force on the robotic arm joint. This equation quantifies the motion response relationship of the robotic arm under external force, providing a dynamic basis for the subsequent calculation of the impedance control mode torque. Then, the impedance dynamics equation, the preset positive definite joint mass matrix, the preset positive definite damping matrix, the preset positive definite stiffness matrix, and the first robotic arm dynamics equation are combined for comprehensive calculation to obtain the impedance control mode torque. This torque enables the robotic arm to protect its structure and maintain motion stability under scenarios with large external forces. Finally, by combining the impedance control mode torque and the preset rotation matrix to obtain the updated rotation matrix, the current spatial posture of the robotic arm under the action of external force can be accurately reflected. Based on the updated rotation matrix, the coordinates of the center of mass of the robotic arm link in the original coordinate system are transformed. Then, combined with the mass of the robotic arm link, the position of the robot's center of mass is obtained to measure the robot's balance state. This avoids the risk of tipping over caused by the displacement of the center of mass due to changes in the position of the robotic arm during the multi-contact balance control process of the humanoid robot. Real-time and precise control of the center of mass position of the humanoid robot is achieved, thereby improving the balance stability of the humanoid robot.

[0009] Furthermore, the calculation of the robot's center of mass position based on the impedance control mode torque, the preset rotation matrix, the coordinates of the robot arm link's center of mass in the original coordinate system, and the mass of the robot arm link includes: Based on the impedance control mode torque, the second robotic arm joint angle is obtained, and the preset rotation matrix is ​​updated based on the second robotic arm joint angle to obtain the second rotation matrix; Based on the second rotation matrix and the coordinates of the center of mass of the robotic arm link in the original coordinate system, calculate the coordinates of the center of mass of the second robotic arm link relative to the ground. The position of the robot's center of mass is calculated based on the ground coordinates of the second robotic arm link's center of mass and the mass of the robotic arm link.

[0010] In the above scheme, the movement of the robotic arm joints is driven by the torque of the impedance control mode, and a second robotic arm joint angle adapted to the current impedance control mode torque is obtained. This accurately reflects the actual joint posture of the robotic arm under large external forces, avoiding deviations from the expected joint posture due to external force interference. Next, the preset rotation matrix is ​​updated based on the second robotic arm joint angle to obtain a second rotation matrix. This second rotation matrix can match the posture changes of the robotic arm under external forces in real time, providing a posture reference that conforms to the current working conditions for the subsequent accurate conversion of the robotic arm link centroid coordinates, ensuring the accuracy of the spatial reference for subsequent centroid calculations. Then, a coordinate transformation operation is performed between the second rotation matrix and the original coordinate system of the robotic arm link centroid coordinates to obtain the ground-based coordinates of the second robotic arm link centroid. This transforms the link centroid coordinates from the original coordinate system to a ground-based coordinate system, thus clearly defining the spatial position of each link centroid in the actual working environment and solving the problem that local coordinates cannot be directly used for the overall robot centroid calculation. Finally, the center of mass of the second robotic arm link is calculated relative to the ground by measuring the mass of the link, thus obtaining the position of the robot's center of mass. This position allows us to measure the balance of the humanoid robot and provides crucial state feedback for its balance control. This enables timely adjustments to the control strategy, avoids the risk of tipping over due to center of mass shift, and achieves precise control of the center of mass position.

[0011] Further, the process of obtaining the first robotic arm dynamic equation, the robotic arm joint angle error, and calculating the robotic arm joint angular velocity error based on the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, the angular velocity of the robotic arm link, the measured robotic arm joint angle, and the measured robotic arm joint angular velocity includes: Based on the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, and the angular velocity of the robotic arm link, the mass matrix, Coriolis centripetal force matrix, and gravity vector are obtained to establish the first robotic arm dynamic equation; Based on the measured joint angles of the robotic arm and the preset target angle function, the joint angle error of the robotic arm is calculated. The joint angular velocity error of the robotic arm is calculated based on the measured joint angular velocity and the preset target angular velocity function.

[0012] In the above scheme, the mass matrix, Coriolis force matrix, and gravity vector are calculated using the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, and the angular velocity of the robotic arm link. A first robotic arm dynamic equation is then established, constructing a mathematical relationship between the joint motion and forces acting on the robotic arm. This model characterizes the joint inertia distribution characteristics, the influence of centrifugal force and Coriolis force on motion, and the effect of gravity on the robotic arm. It also serves as the theoretical basis for subsequent compensation calculations and torque derivation, providing theoretical support for the stable control of the robotic arm under complex working conditions. Next, the difference between the measured robotic arm joint angles and the preset target angle function is calculated to obtain the robotic arm joint angle error. This quantifies the degree of deviation between the actual and ideal positions of the robotic arm joints, serving as a basis for correcting positional deviations in subsequent control compensation. Then, the difference between the measured angular velocity of the robotic arm joint and the preset target angular velocity function is calculated to obtain the angular velocity error of the robotic arm joint. This clarifies the deviation between the actual movement speed and the ideal speed of the robotic arm joint. Together with the obtained angular velocity error of the robotic arm joint, it constitutes the key input for control compensation, so as to adjust the dynamic characteristics of the subsequent torque output in a targeted manner, ensuring the smooth movement and accurate speed of the robotic arm, and laying the foundation for obtaining a stable control torque in the future.

[0013] Further, the process of obtaining the mass matrix, Coriolis centripetal force matrix, and gravity vector based on the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, and the angular velocity of the robotic arm link, to establish the first robotic arm dynamic equation, includes: Based on the preset rotation matrix and the coordinates of the center of mass of the robotic arm link in the original coordinate system, calculate the initial coordinates of the center of mass of the robotic arm link relative to the ground. Based on the initial coordinates of the center of mass of the robotic arm link relative to the ground, obtain the linear velocity of the center of mass of the robotic arm link relative to the ground. Based on the mass of the robotic arm links, obtain the inertia tensor; The total kinetic energy is obtained based on the mass of the robotic arm link, the linear velocity of the center of mass of the robotic arm link relative to the ground, the angular velocity of the robotic arm link, and the inertia tensor. The total potential energy is obtained based on the mass of the robotic arm link and the height of the robotic arm link's center of mass relative to the ground. Based on the total kinetic energy and total potential energy, the mass matrix, Coriolis centripetal force matrix, and gravity vector are obtained, and the first dynamic equation of the robotic arm is established.

[0014] In the above scheme, a coordinate transformation operation is performed between a preset rotation matrix and the coordinates of the manipulator's link center of mass in the original coordinate system to obtain the initial coordinates of the manipulator's link center of mass relative to the ground. This converts the coordinates of the center of mass in the original coordinate system to coordinates in the global ground coordinate system, clarifying the initial spatial positioning of each link's center of mass in the actual working environment. This provides a global coordinate reference for subsequent calculations of parameters such as the linear velocity of the center of mass and total kinetic energy, ensuring spatial consistency in subsequent calculations. Next, the linear velocity of the manipulator's link center of mass relative to the ground is obtained from the initial coordinates of the manipulator's link center of mass relative to the ground. This reflects the speed and direction of the manipulator's link center of mass's motion in global space, and its accuracy directly affects the subsequent calculation results of total kinetic energy, providing data support for constructing the dynamic model. Finally, by obtaining the inertia tensor of the manipulator's link, key parameters for the mass distribution dimension are provided for subsequent calculations of total kinetic energy and construction of dynamic equations. Subsequently, the total kinetic energy is obtained by measuring the mass of the robotic arm links, the linear velocity of the center of mass of the robotic arm links relative to the ground, the angular velocity of the robotic arm links, and the inertia tensor. This total kinetic energy comprehensively reflects the overall kinetic energy state of the robotic arm, providing key data support in the energy dimension for establishing the dynamic equations and is one of the core components of constructing the dynamic model. Furthermore, the total potential energy is obtained by measuring the mass of the robotic arm links and the height of the center of mass of the robotic arm links relative to the ground. This reflects the potential energy state of the robotic arm in the gravitational field, providing data support in the gravitational potential energy dimension for constructing the dynamic equations and ensuring that the dynamic model accurately reflects the influence of gravity on the robotic arm's motion. Finally, the mass matrix, Coriolis centripetal force matrix, and gravity vector were derived from the total kinetic energy and total potential energy. The mass matrix reflects the influence of joint inertial characteristics on joint acceleration, the Coriolis centripetal force matrix reflects the coupling effect of joint angular velocity on joint torque, and the gravity vector represents the force of gravity on the joint. Finally, the first dynamic equation of the robotic arm was established, clarifying the mathematical relationship between the joint motion and joint torque of the robotic arm. This provides a core theoretical model for subsequent torque calculation under different control modes, serving as the foundation for achieving precise control of the robotic arm.

[0015] Furthermore, the determination of state feedback compensation and state observer compensation based on the joint angle error, joint angular velocity error, and the first robotic arm dynamics equation includes: Based on the preset eigenvalue matrix, preset free matrix, robotic arm joint angle error, robotic arm joint angular velocity error and the first robotic arm dynamic equation, state feedback compensation is obtained; Based on the preset diagonal matrix, the angular velocity error of the robotic arm joint, the gravity vector, the state feedback compensation, and the preset interference observation auxiliary variables, the state observer compensation is determined.

[0016] In the above scheme, state feedback compensation is obtained by pre-setting eigenvalue matrices and pre-setting free matrices, combined with the joint angle error, joint angular velocity error, and the first robotic arm dynamic equation. This state feedback compensation corrects the deviation between the joint angle and angular velocity of the robotic arm, providing a basic compensation basis for the calculation of torque in the position control mode. Next, state observer compensation is calculated and determined by pre-setting diagonal matrices, robotic arm joint angular velocity errors, gravity vectors, state feedback compensation, and pre-setting disturbance observation auxiliary variables. This compensates for internal and external uncertainties in the robotic arm, preventing disturbances from causing control deviations and motion instability. Together with state feedback compensation, this improves the robustness of the position control mode.

[0017] Furthermore, the process of obtaining state feedback compensation based on a preset eigenvalue matrix, a preset free matrix, robotic arm joint angle errors, robotic arm joint angular velocity errors, and the first robotic arm dynamic equation includes: Calculate the finite eigenvector matrix and parameter matrix based on the preset eigenvalue matrix and preset free matrix; The feedback gain matrix is ​​obtained based on the finite eigenvector matrix and parameter matrix; A state vector is constructed based on the joint angle error and joint angular velocity error of the robotic arm. Based on the preset identity matrix, feedback gain matrix, mass matrix, and Coriolis centripetal force matrix, construct the first state matrix and input matrix; Based on the first state matrix, the state vector, and the input matrix, obtain the input vector; State feedback compensation is obtained based on the input vector, feedback gain matrix, robotic arm joint angle error, robotic arm joint angular velocity error, preset identity matrix, and gravity vector.

[0018] In the above scheme, a finite eigenvector matrix and a parameter matrix are obtained by pre-setting an eigenvalue matrix and a pre-setting free matrix. These finite eigenvector matrix and parameter matrix serve as the core intermediate carriers for deriving the feedback gain matrix, providing a mathematical foundation for subsequent acquisition and ensuring that the feedback gain calculation meets the stable convergence requirements. Next, the feedback gain matrix is ​​obtained from the finite eigenvector matrix and parameter matrix, serving as the core parameter for state feedback compensation. This avoids motion oscillations due to excessive gain or slow error convergence due to insufficient gain, providing a gain basis for subsequent correction of robotic arm joint deviations. Then, a state vector is constructed using the robotic arm joint angle error and the robotic arm joint angular velocity error, reflecting the deviation between the current motion state and the ideal state. This facilitates subsequent analysis and compensation calculations, providing a unified input form for state feedback control. Subsequently, an input vector is obtained from the first state matrix and the input matrix. This input vector transforms the linearized control requirements into specific mathematical quantities, providing a basis for subsequent calculations of state feedback compensation and ensuring that the compensation amount meets the requirements. Finally, the state feedback compensation is calculated and used as the core control quantity for correcting robotic arm joint deviations, providing a correction basis for torque calculation in the position control mode and ensuring that the robotic arm moves smoothly along the ideal trajectory.

[0019] Furthermore, the determination of state observer compensation based on a preset diagonal matrix, robotic arm joint angular velocity error, gravity vector, state feedback compensation, and preset interference observation auxiliary variables includes: Based on the preset diagonal matrix and the angular velocity error of the robotic arm joints, the observer gain matrix is ​​obtained; Based on the observer gain matrix, gravity vector, state feedback compensation, and preset interference observation auxiliary variables, the interference observation values ​​are calculated, and then the state observer compensation is determined.

[0020] In the above scheme, the stability of the observer is ensured by pre-setting a diagonal matrix. The observer gain matrix is ​​obtained by combining the angular velocity error of the robotic arm joints, allowing the gain matrix to adapt to the current motion dynamics. This avoids the observer's response to rapidly changing errors being lagging or overshooting due to a fixed gain, laying the gain foundation for subsequent accurate estimation of interference observations. Then, the observer gain matrix ensures the speed and accuracy of interference estimation. Combined with gravity vector and state feedback compensation, the influence of known interference and measurable errors is eliminated. Finally, interference observations are obtained based on pre-set interference observation auxiliary variables, ensuring that these observations only reflect the magnitude and trend of unknown interference, and are ultimately determined as state observer compensation.

[0021] Furthermore, the calculation of the robot's center of mass position based on the position control mode torque, the preset rotation matrix, the coordinates of the robot arm link's center of mass in the original coordinate system, and the mass of the robot arm link includes: Based on the position control mode torque, the first robotic arm joint angle is obtained, and the preset rotation matrix is ​​updated based on the first robotic arm joint angle to obtain the first rotation matrix; Based on the first rotation matrix and the coordinates of the center of mass of the robotic arm link in the original coordinate system, calculate the coordinates of the center of mass of the first robotic arm link relative to the ground. The position of the robot's center of mass is calculated based on the ground coordinates of the first robotic arm link's center of mass and the mass of the robotic arm link.

[0022] In the above scheme, the robotic arm is driven by torque in the position control mode to obtain the first joint angle of the robotic arm that matches the torque of the current position control mode. This can accurately reflect the actual joint posture of the robotic arm moving along the preset trajectory, avoiding deviation of the joint posture from the ideal state due to torque output deviation. Then, the preset rotation matrix is ​​updated based on the first joint angle of the robotic arm to obtain the first rotation matrix. The updated first rotation matrix can match the joint posture changes of the robotic arm in the position control mode in real time, eliminate the deviation between the preset rotation matrix and the actual posture, and provide a posture reference that conforms to the current motion state for the subsequent conversion of the centroid coordinates of the robotic arm link, ensuring that the spatial reference for the centroid calculation is consistent with the actual working condition of the robotic arm. Next, a coordinate transformation operation is performed using the first rotation matrix and the coordinates of the manipulator's link centroid in the original coordinate system to obtain the ground coordinates of the first manipulator link centroid. This achieves a coordinate transformation from the manipulator's local coordinate system to the global coordinate system on the ground. The original coordinate system, which only described the relative positions of the links, is transformed into global coordinates that clearly define the absolute spatial positions of each link in the actual working environment. This solves the problem that the original coordinate system's manipulator link centroid coordinates cannot be directly used for calculating the robot's overall centroid, providing accurate positional data for the final calculation of the robot's centroid position. Finally, calculations are performed using the mass of each link of the manipulator and the ground coordinates of the first manipulator link centroid. This allows for real-time capture of centroid offsets caused by posture changes during the manipulator's trajectory movement, avoiding the neglect of centroid calculation errors due to mass differences. The final centroid position of the robot is obtained to measure its balance state, thus avoiding the risk of tipping over in multi-contact balance control of humanoid robots caused by centroid offsets due to manipulator position changes. This achieves real-time and accurate control of the humanoid robot's centroid position, thereby improving the robot's balance stability.

[0023] This invention provides a robot center of mass position calculation system, including a parameter acquisition module, an external force acquisition module, a control mode decision module, and a center of mass position calculation module, specifically: The parameter acquisition module is used to acquire the mass of several robotic arm links, the height of the center of mass of several robotic arm links relative to the ground, the coordinates of the center of mass of several robotic arm links in the original coordinate system, the angular velocity of several robotic arm links, the measured joint angle of the robotic arm, the measured joint angular velocity of the robotic arm, and the measured external force of the robotic arm joint. The external force acquisition module is used to obtain the first mechanical arm dynamic equation, the mechanical arm joint angle error and calculate the mechanical arm joint angular velocity error based on the mechanical arm link mass, the height of the mechanical arm link center of mass relative to the ground, the coordinates of the mechanical arm link center of mass in the original coordinate system, the mechanical arm link angular velocity, the measured mechanical arm joint angle and the measured mechanical arm joint angular velocity. The control mode decision module is used to execute position control mode operation and obtain position control mode torque if the measured external force of the robotic arm joint is less than or equal to a preset external force threshold. The centroid position calculation module is used to calculate the robot's centroid position based on the position control mode torque, the preset rotation matrix, the centroid coordinates of the robot arm link in the original coordinate system, and the mass of the robot arm link. The position control mode operation includes: Based on the joint angle error of the robotic arm, the joint angular velocity error of the robotic arm, and the first dynamic equation of the robotic arm, the state feedback compensation and the state observer compensation are determined. The position control mode torque is obtained based on state feedback compensation and state observer compensation.

[0024] This invention provides a robot centroid position calculation system. In practical applications, it only requires an external force acquisition module. By establishing a first robotic arm dynamic equation, the mathematical relationship between the robotic arm joint motion and the applied force is clarified, providing a theoretical model for control compensation calculation. Simultaneously, the joint angle error and joint angular velocity error of the robotic arm are calculated, quantifying the deviation between the actual and ideal states, providing a basis for error correction in position control mode. Next, a control mode decision module is used. When the measured external force on the robotic arm joint is less than or equal to a preset external force threshold, the position control mode operation is triggered, ensuring that the control mode is adapted to the actual force conditions of the robotic arm. Then, based on the joint angle error, joint angular velocity error, and the first robotic arm dynamic equation, the position control mode torque is obtained. This position control mode torque ensures the motion accuracy and stability of the robotic arm in low-force scenarios. Finally, the center of mass position calculation module is used to obtain the current spatial posture of the robot arm by combining the position control mode torque with the preset rotation matrix. Based on the updated rotation matrix, the center of mass coordinates of the robot arm links in the original coordinate system are transformed. Then, the robot's center of mass position is obtained by combining the mass of the robot arm links to measure the robot's balance state. This avoids the risk of tipping over caused by the center of mass shift due to changes in the position of the robot arm during multi-contact balance control of the humanoid robot. It achieves real-time and accurate control of the center of mass position of the humanoid robot, thereby improving the balance stability of the humanoid robot. Attached Figure Description

[0025] Figure 1 A flowchart illustrating a method for calculating the center of mass position of a robot according to an embodiment of the present invention; Figure 2 This is an architecture diagram of a robot's center of mass position calculation system provided in an embodiment of the present invention. Detailed Implementation

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

[0027] This embodiment provides a method for calculating the center of mass position of a robot; please refer to the flowchart for details. Figure 1 ,include: Step S1: Obtain the mass of several robotic arm links, the height of the center of mass of several robotic arm links relative to the ground, the coordinates of the center of mass of several robotic arm links in the original coordinate system, the angular velocity of several robotic arm links, the measured joint angle of the robotic arm, the measured joint angular velocity of the robotic arm, and the measured external force of the robotic arm joint; Step S2: Based on the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, the angular velocity of the robotic arm link, the measured robotic arm joint angle, and the measured robotic arm joint angular velocity, obtain the first robotic arm dynamic equation, the robotic arm joint angle error, and calculate the robotic arm joint angular velocity error. Step S3: If the measured external force of the robotic arm joint is less than or equal to the preset external force threshold, execute the position control mode operation and obtain the position control mode torque; Step S4: Calculate the position of the robot's center of mass based on the position control mode torque, preset rotation matrix, coordinates of the robot arm link's center of mass in the original coordinate system, and the mass of the robot arm link. The position control mode operation includes: Based on the joint angle error of the robotic arm, the joint angular velocity error of the robotic arm, and the first dynamic equation of the robotic arm, the state feedback compensation and the state observer compensation are determined. The position control mode torque is obtained based on state feedback compensation and state observer compensation.

[0028] In this embodiment, the robotic arm is a seven-degree-of-freedom robotic arm, and a six-axis sensor is installed at the end of the robotic arm to acquire the measured external forces on the robotic arm joints. The sensor also acquires the following data: the mass of several robotic arm links, the height of the center of mass of several robotic arm links relative to the ground, the coordinates of the center of mass of several robotic arm links in the original coordinate system, and the angular velocities of several robotic arm links. ( ,in It is the positive direction of the rotation axis of the robotic arm joint u. It is the angular velocity of link u in coordinate system i. This includes the magnitude of the angular velocity of link u+1, the measured joint angle of the robotic arm, the measured joint angular velocity of the robotic arm, and the measured external force on the robotic arm joint. By establishing the first dynamic equation of the robotic arm, the mathematical relationship between the joint motion and the force acting on the robotic arm is clarified, providing a theoretical model for control compensation calculations. Simultaneously, the joint angle error and joint angular velocity error of the robotic arm are calculated, quantifying the deviation between the actual and ideal states, and providing a basis for error correction in position control mode. Next, when the measured external force on the robotic arm joint... When the external force is less than or equal to the preset external force threshold C, the position control mode is triggered to ensure that the control mode is adapted to the actual force conditions of the robotic arm. Then, based on the joint angle error, joint angular velocity error, and the first robotic arm dynamics equation, a second-order all-drive method is used for control. ,in, This represents status feedback compensation. This represents compensation by the state observer, thereby obtaining the torque of the position control mode. ( By using the position control mode torque, the motion accuracy and stability of the robotic arm can be ensured in low-force scenarios. Finally, the spatial posture of the current robotic arm is obtained by combining the position control mode torque with a preset rotation matrix. Based on the updated rotation matrix, the coordinates of the center of mass of the robotic arm links in the original coordinate system are transformed. Then, combined with the mass of the robotic arm links, the position of the robot's center of mass is obtained to measure the robot's balance state. This avoids the risk of tipping over due to center of mass shift caused by changes in the position of the robotic arm during multi-contact balance control of the humanoid robot, achieving real-time and precise control of the humanoid robot's center of mass position, thereby improving the balance stability of the humanoid robot.

[0029] Furthermore, it also includes: If the measured external force on the robotic arm joint is greater than the preset external force threshold, the impedance control mode operation is executed to obtain the impedance control mode torque; Based on the impedance control mode torque, the preset rotation matrix, the coordinates of the robot arm link's center of mass in the original coordinate system, and the mass of the robot arm link, the position of the robot's center of mass is calculated. The impedance control mode operation includes: Based on the joint angle error, joint angular velocity error, preset positive definite joint mass matrix, preset positive definite damping matrix, preset positive definite stiffness matrix, and measured external force of the joint, an impedance dynamics equation is established. Based on the impedance dynamics equation and the first robotic arm dynamics equation, the torque of the impedance control mode is obtained.

[0030] In this embodiment, if the external force of the robotic arm joint is measured... If the force exceeds the preset external force threshold C, the impedance control mode is triggered to ensure that the control mode is compatible with the working conditions where the robotic arm is subjected to large external forces, thus preventing damage to the robotic arm joints or the wall surface due to overload. In impedance control mode, the system first considers the robotic arm joint angle error, the robotic arm joint angular velocity error, and the preset positive definite robotic arm joint mass matrix. Preset positive definite damping matrix Preset positive definite stiffness matrix Establish the impedance dynamics equation based on the measured external forces at the robotic arm joints: ,in, For the joint angle error of the robotic arm, For the angular velocity error of the robotic arm joints, The error in the joint angular acceleration of the robotic arm (achieved by comparing the measured joint angular acceleration of the robotic arm with the target joint angular acceleration). The difference is calculated to obtain the measured angular acceleration of the robotic arm joints (obtained by differentiating the measured angular velocity of the robotic arm joints with respect to time; the target robotic arm joint angular acceleration is a preset value). This is used to quantify the motion response of the robotic arm under external force, providing a dynamic basis for the subsequent calculation of the impedance control mode torque. Next, a comprehensive calculation is performed combining the impedance dynamics equation and the first robotic arm dynamics equation to obtain the impedance control mode torque: substituting the mass matrix, Coriolis centripetal force matrix, and gravity vector from the first robotic arm dynamics equation yields the impedance control mode torque. ,in, 7 The identity matrix of 7, for The inverse matrix, where q is the measured joint angle of the robotic arm. To measure the joint angular velocity of the robotic arm, For the quality matrix, The Coriolis centripetal force matrix, This is the gravity vector. This torque enables the robotic arm to protect its structure and maintain motion stability under significant external forces. Finally, by combining the impedance control mode torque and the preset rotation matrix, an updated rotation matrix is ​​obtained. This updated rotation matrix accurately reflects the current spatial posture of the robotic arm under external forces. Based on the updated rotation matrix, the coordinates of the robotic arm link's center of mass in the original coordinate system are transformed. Then, combined with the mass of the robotic arm link, the position of the robot's center of mass is obtained to measure the robot's balance state. This avoids the risk of tipping over due to center of mass shift caused by changes in the robotic arm's position during multi-contact balance control of humanoid robots. It achieves real-time and precise control of the humanoid robot's center of mass position, thereby improving the robot's balance stability.

[0031] Furthermore, the calculation of the robot's center of mass position based on the impedance control mode torque, the preset rotation matrix, the coordinates of the robot arm link's center of mass in the original coordinate system, and the mass of the robot arm link includes: Based on the impedance control mode torque, the second robotic arm joint angle is obtained, and the preset rotation matrix is ​​updated based on the second robotic arm joint angle to obtain the second rotation matrix; Based on the second rotation matrix and the coordinates of the center of mass of the robotic arm link in the original coordinate system, calculate the coordinates of the center of mass of the second robotic arm link relative to the ground. The position of the robot's center of mass is calculated based on the ground coordinates of the second robotic arm link's center of mass and the mass of the robotic arm link.

[0032] In this embodiment, the robotic arm joints are driven by impedance control mode torque to obtain a second robotic arm joint angle that matches the current impedance control mode torque. This accurately reflects the actual joint posture of the robotic arm under conditions of significant external force, preventing deviations from the expected joint posture due to external force interference. Next, the preset rotation matrix is ​​updated based on the second robotic arm joint angle to obtain the second rotation matrix. This allows for real-time matching of the robotic arm's posture changes under external forces, providing a posture reference consistent with the current working conditions for accurate subsequent conversion of the robotic arm link's center of mass coordinates, ensuring the accuracy of the spatial reference for subsequent center of mass calculations. Then, the second rotation matrix is ​​used to compare the robotic arm link's center of mass coordinates with those in the original coordinate system. Perform coordinate transformation operations: The coordinates of the center of mass of the second robotic arm link relative to the ground were obtained. These coordinates were then transformed from the original coordinate system to a ground-based coordinate system, thus clearly defining the spatial position of each link's center of mass in the actual working environment and solving the problem that local coordinates cannot be directly used for calculating the robot's overall center of mass. Finally, the mass of each link of the robotic arm was used to... Calculate the coordinates of the center of mass of the second robotic arm link relative to the ground: The position of the robot's center of mass is obtained. (Actually a 3) (Vector), thus enabling the measurement of the humanoid robot's balance state through the robot's center of mass position, providing crucial state feedback for the humanoid robot's balance control, thereby allowing timely adjustment of control strategies, avoiding the risk of tipping over due to center of mass shift, and achieving precise control of the center of mass position.

[0033] Further, the process of obtaining the first robotic arm dynamic equation, the robotic arm joint angle error, and calculating the robotic arm joint angular velocity error based on the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, the angular velocity of the robotic arm link, the measured robotic arm joint angle, and the measured robotic arm joint angular velocity includes: Based on the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, and the angular velocity of the robotic arm link, the mass matrix, Coriolis centripetal force matrix, and gravity vector are obtained to establish the first robotic arm dynamic equation; Based on the measured joint angles of the robotic arm and the preset target angle function, the joint angle error of the robotic arm is calculated. The joint angular velocity error of the robotic arm is calculated based on the measured joint angular velocity and the preset target angular velocity function.

[0034] In this embodiment, the mass matrix, Coriolis force matrix, and gravity vector are calculated using the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, and the angular velocity of the robotic arm link. A first robotic arm dynamic equation is then established, constructing a mathematical relationship between the joint motion and forces acting on the robotic arm. This model characterizes the joint inertia distribution characteristics, the influence of centrifugal force and Coriolis force on motion, and the effect of gravity on the robotic arm. It also serves as the theoretical basis for subsequent compensation calculations and torque derivation, providing theoretical support for the stable control of the robotic arm under complex working conditions. Next, the measured robotic arm joint angle q is compared with a preset target angle function. Perform the difference calculation to obtain the joint angle error of the robotic arm. This quantifies the deviation between the actual and ideal positions of the robotic arm joints, providing a basis for correcting positional deviations in subsequent control compensation. Then, the measured angular velocities of the robotic arm joints are... With the preset target angular velocity function Perform the difference calculation to obtain the joint angular velocity error of the robotic arm. This clarifies the deviation between the actual and ideal speeds of the robotic arm joints. This deviation, together with the aforementioned joint angle error, constitutes a key input for control compensation. This allows for targeted adjustments to the dynamic characteristics of subsequent torque output, ensuring smooth movement and accurate speed of the robotic arm, and laying the foundation for obtaining stable control torque in the future.

[0035] Further, the process of obtaining the mass matrix, Coriolis centripetal force matrix, and gravity vector based on the mass of the robotic arm link, the height of the robotic arm link's center of mass relative to the ground, the coordinates of the robotic arm link's center of mass in the original coordinate system, and the angular velocity of the robotic arm link, to establish the first robotic arm dynamic equation, includes: Based on the preset rotation matrix and the coordinates of the center of mass of the robotic arm link in the original coordinate system, calculate the initial coordinates of the center of mass of the robotic arm link relative to the ground. Based on the initial coordinates of the center of mass of the robotic arm link relative to the ground, obtain the linear velocity of the center of mass of the robotic arm link relative to the ground. Based on the mass of the robotic arm links, obtain the inertia tensor; The total kinetic energy is obtained based on the mass of the robotic arm link, the linear velocity of the center of mass of the robotic arm link relative to the ground, the angular velocity of the robotic arm link, and the inertia tensor. The total potential energy is obtained based on the mass of the robotic arm link and the height of the robotic arm link's center of mass relative to the ground. Based on the total kinetic energy and total potential energy, the mass matrix, Coriolis centripetal force matrix, and gravity vector are obtained, and the first dynamic equation of the robotic arm is established.

[0036] In this embodiment, for any robotic arm joint u in the original coordinate system In the middle, its center of mass is And let the rotation matrix between the two coordinate systems be . Thus, the preset rotation matrix is ​​obtained. (That is, the rotation matrix from the ground coordinate system to the original coordinate system of each joint). Coordinate transformation is performed using the preset rotation matrix and the coordinates of the robot arm's link centroid in the original coordinate system: The initial coordinates of the center of mass of the robotic arm link relative to the ground are obtained. The coordinates of the center of mass in the original coordinate system were converted to coordinates in the global ground coordinate system, clarifying the initial spatial positioning of the center of mass of each link in the actual working environment. This provides a global coordinate reference for subsequent calculations of parameters such as the linear velocity of the center of mass and total kinetic energy, ensuring spatial consistency in subsequent calculations. Next, the time derivative of the initial coordinates of the robot arm link's center of mass relative to the ground was calculated: Obtain the linear velocity of the robotic arm link's center of mass relative to the ground. This reflects the speed and direction of the robotic arm link's center of mass movement in global space, and its accuracy directly affects the subsequent total kinetic energy calculation results, providing data support for constructing the dynamic model. Then, based on the three-dimensional distribution of the robotic arm link's mass, the inertia tensor of the robotic arm link is obtained: , , , , , , Where x represents the mass distribution of the robotic arm links along the x-axis, y represents the mass distribution of the robotic arm links along the y-axis, and z represents the mass distribution of the robotic arm links along the z-axis. , and The moment of inertia of a rigid body about its mass axes. , and For the inertial product, to simplify the Lagrange dynamics calculation, only the product of the main diagonal of the inertial tensor is taken. , and Calculations are performed to obtain the inertia tensor of the robotic arm links. ，This provides key parameters for the mass distribution dimension, which are crucial for subsequent calculations of the total kinetic energy and the construction of the dynamic equations. Subsequently, the total kinetic energy is obtained using the mass of the robotic arm link, the linear velocity of the robotic arm link's center of mass relative to the ground, the angular velocity of the robotic arm link, and the inertia tensor: the total kinetic energy can be expressed as the sum of the translational kinetic energy and the rotational kinetic energy. For translational kinetic energy, For rotational kinetic energy, Let the mass of the link of the robotic arm u be... Let U be the linear velocity of the center of mass of the robotic arm's U-link relative to the ground. Let be the angular velocity of the robotic arm link u in coordinate system i. This total kinetic energy comprehensively reflects the overall kinetic energy state of the robotic arm, providing key energy dimension data support for establishing the dynamic equations and is one of the core components of constructing the dynamic model. Furthermore, the total potential energy is obtained through the mass of the robotic arm link and the height of the robotic arm link's center of mass relative to the ground: g is the acceleration due to gravity. The height of the center of mass of the link of the robotic arm u relative to the ground reflects the potential energy state of the robotic arm in the gravitational field, providing data support for the construction of the dynamic equations in the dimension of gravitational potential energy, and ensuring that the dynamic model can accurately reflect the influence of gravity on the motion of the robotic arm. Finally, the mass matrix, Coriolis centripetal force matrix, and gravity vector are derived from the total kinetic energy and total potential energy through the Lagrange dynamic equations: In the formula, the Lagrange function Generalized coordinates Let d be the joint angle of the robotic arm, and d be the output torque of the robot controller. ,in, 7 A mass matrix of 7 7 The Coriolis centripetal force matrix of 7, Let d be a 7×1 gravity vector, and d be a 7×1 controller output torque vector. Represents the angles of each joint of the robotic arm. Represents the angular velocity of each joint of the robotic arm. Represents the angular acceleration of each joint of the robotic arm. To measure the external forces at the joints of the robotic arm, the mass matrix reflects the influence of joint inertia on joint acceleration, the Coriolis centripetal force matrix illustrates the coupling effect of joint angular velocity on joint torque, and the gravity vector characterizes the force exerted by gravity on the joint. Finally, the first dynamic equation of the robotic arm is established, clarifying the mathematical relationship between joint motion and joint torque. This provides a core theoretical model for subsequent torque calculations under different control modes, serving as the foundation for achieving precise control of the robotic arm.

[0037] Furthermore, the determination of state feedback compensation and state observer compensation based on the joint angle error, joint angular velocity error, and the first robotic arm dynamics equation includes: Based on the preset eigenvalue matrix, preset free matrix, robotic arm joint angle error, robotic arm joint angular velocity error and the first robotic arm dynamic equation, state feedback compensation is obtained; Based on the preset diagonal matrix, the angular velocity error of the robotic arm joint, the gravity vector, the state feedback compensation, and the preset interference observation auxiliary variables, the state observer compensation is determined.

[0038] In this embodiment, when the external force of the robotic arm joint is measured... When the force is less than or equal to a preset external force threshold C, the position control mode is triggered. State feedback compensation is obtained by combining the preset eigenvalue matrix, the preset free matrix, the robot arm joint angle error, the robot arm joint angular velocity error, and the first robot arm dynamics equation. This allows for the correction of deviations between the robot arm joint angles and angular velocities through state feedback compensation, providing a fundamental compensation basis for calculating the torque in the position control mode. Next, by pre-setting a diagonal matrix, robot arm joint angular velocity error, gravity vector, state feedback compensation, and pre-set disturbance observation auxiliary variables, the state observer compensation is calculated and determined to compensate for internal and external uncertainties in the robot arm, preventing disturbances from causing control deviations and motion instability. This, together with state feedback compensation, enhances the robustness of the position control mode.

[0039] Furthermore, the process of obtaining state feedback compensation based on a preset eigenvalue matrix, a preset free matrix, robotic arm joint angle errors, robotic arm joint angular velocity errors, and the first robotic arm dynamic equation includes: Calculate the finite eigenvector matrix and parameter matrix based on the preset eigenvalue matrix and preset free matrix; The feedback gain matrix is ​​obtained based on the finite eigenvector matrix and parameter matrix; A state vector is constructed based on the joint angle error and joint angular velocity error of the robotic arm. Based on the preset identity matrix, feedback gain matrix, mass matrix, and Coriolis centripetal force matrix, construct the first state matrix and input matrix; Based on the first state matrix, the state vector, and the input matrix, obtain the input vector; State feedback compensation is obtained based on the input vector, feedback gain matrix, robotic arm joint angle error, robotic arm joint angular velocity error, preset identity matrix, and gravity vector.

[0040] In this embodiment, the state feedback controller is: In the formula, For the preset identity matrix, and Let be the feedback gain matrix to be determined. For the joint angle error of the robotic arm, For the angular velocity error of the robotic arm joints, It is the input vector. A finite eigenvector matrix and parameter matrix are obtained through a preset eigenvalue matrix and a preset free matrix: through... Given the given conditions, construct a preset eigenvalue matrix F (a 14×14 diagonal matrix, where all elements on the main diagonal of F lie in the negative half-plane) and a preset free matrix Z; then obtain a finite eigenvector matrix using the preset eigenvalue matrix F and the preset free matrix Z. and parameter matrix : , , These are preset coefficients, which can be set empirically. The finite eigenvector matrix and parameter matrix serve as the core intermediate carriers for deriving the feedback gain matrix, providing the mathematical foundation for subsequently obtaining the feedback gain matrix and ensuring that the feedback gain calculation meets the stable convergence requirement. Next, the feedback gain matrix is ​​obtained through the finite eigenvector matrix and parameter matrix: As the core parameter for state feedback compensation, it can prevent motion oscillations caused by excessive gain or slow error convergence caused by insufficient gain, providing a gain basis for subsequent correction of robot arm joint deviations. Then, the state vector X is constructed using the robot arm joint angle error and the robot arm joint angular velocity error: This reflects the deviation between the current motion state of the robotic arm and the ideal state, facilitating subsequent analysis and compensation calculations, and providing a unified input format for state feedback control. Based on the preset identity matrix, feedback gain matrix, mass matrix, and Coriolis centripetal force matrix, the first state matrix and input matrix are constructed: First State Matrix Input matrix ,in M is the predefined identity matrix, and M is the mass matrix. For the preset identity matrix, Where K=0, and The first state matrix The matrix that constitutes the structure Where D is the Coriolis centripetal force matrix. Then, the input vector is obtained using the first state matrix and the input matrix: This input vector can be used to transform the linearized control requirements into specific mathematical quantities, providing a basis for subsequent calculations of state feedback compensation and ensuring that the compensation amount meets the requirements. Finally, the state feedback compensation is calculated. As the core control quantity for correcting joint deviations of the robotic arm, it can provide a basis for correcting torque calculations in position control mode, ensuring that the robotic arm moves smoothly along the ideal trajectory.

[0041] Furthermore, the determination of state observer compensation based on a preset diagonal matrix, robotic arm joint angular velocity error, gravity vector, state feedback compensation, and preset interference observation auxiliary variables includes: Based on the preset diagonal matrix and the angular velocity error of the robotic arm joints, the observer gain matrix is ​​obtained; Based on the observer gain matrix, gravity vector, state feedback compensation, and preset interference observation auxiliary variables, the interference observation values ​​are calculated, and then the state observer compensation is determined.

[0042] In this embodiment, the stability of the observer is ensured by a preset diagonal matrix, and the observer gain matrix L is obtained by combining the joint angular velocity error of the robotic arm: ,in, For the angular velocity error of the robotic arm joints, This is a pre-set diagonal matrix. It allows the observer gain matrix to adapt to the current motion dynamics, avoiding lag or overshoot in the observer's response to rapidly changing errors due to a fixed gain, thus laying the gain foundation for subsequent accurate estimation of disturbance observations. Then, based on the observer gain matrix, gravity vector, state feedback compensation, and pre-set disturbance observation auxiliary variables, the disturbance observation values ​​are calculated: Where z is a preset auxiliary variable for interference observation. To interfere with the observed values, The Coriolis centripetal force matrix, As an intermediate variable, Using the gravity vector, the observer gain matrix ensures the speed and accuracy of disturbance estimation. Combining the gravity vector and state feedback compensation, the influence of known disturbances and measurable errors is eliminated. Then, based on preset disturbance observation auxiliary variables, disturbance observation values ​​are obtained, ensuring that these observation values ​​only reflect the magnitude and trend of unknown disturbances. Finally, these are determined as the state observer compensation. .

[0043] Furthermore, the calculation of the robot's center of mass position based on the position control mode torque, the preset rotation matrix, the coordinates of the robot arm link's center of mass in the original coordinate system, and the mass of the robot arm link includes: Based on the position control mode torque, the first robotic arm joint angle is obtained, and the preset rotation matrix is ​​updated based on the first robotic arm joint angle to obtain the first rotation matrix; Based on the first rotation matrix and the coordinates of the center of mass of the robotic arm link in the original coordinate system, calculate the coordinates of the center of mass of the first robotic arm link relative to the ground. The position of the robot's center of mass is calculated based on the ground coordinates of the first robotic arm link's center of mass and the mass of the robotic arm link.

[0044] In this embodiment, the torque is controlled by the position control mode. Drive the robotic arm to move and obtain the first robotic arm joint angle that matches the torque of the current position control mode. It can accurately reflect the actual joint posture of the robotic arm moving along a preset trajectory, avoiding deviations in joint posture from the ideal state due to torque output errors; then, based on the first robotic arm joint angle, the preset rotation matrix is ​​updated to obtain the first rotation matrix. The updated first rotation matrix can match the joint posture changes of the robotic arm in real time under position control mode, eliminating the deviation between the preset rotation matrix and the actual posture. This provides a posture reference that conforms to the current motion state for subsequent conversion of the robotic arm link centroid coordinates, ensuring that the spatial reference for centroid calculation is consistent with the actual working condition of the robotic arm. Next, the first rotation matrix and the robotic arm link centroid coordinates in the original coordinate system are used to... Perform coordinate transformation operations: The coordinates of the center of mass of the first robotic arm link relative to the ground were obtained. This method achieves coordinate transformation from the local coordinate system of the robotic arm to the global coordinate system of the ground. It transforms the original coordinate system, which only described the relative positions of the links, into global coordinates that clearly define the absolute spatial position of each link in the actual working environment. This solves the problem that the original coordinate system's link centroid coordinates cannot be directly used for calculating the robot's overall centroid, providing accurate position data for the final calculation of the robot's centroid position. Finally, based on the mass of each link of the robotic arm... Calculate the coordinates of the center of mass of the first robotic arm link relative to the ground: It can capture the center of mass shift caused by the change in posture of the robotic arm as it moves along the trajectory in real time, avoiding the deviation in center of mass calculation caused by ignoring mass differences, and finally obtain the position of the robot's center of mass. (Actually a 3) Vectors are used to measure the robot's balance state, thereby avoiding the risk of tipping over due to the shift in the center of mass caused by changes in the position of the robotic arm during multi-contact balance control of humanoid robots. This enables real-time and precise control of the center of mass position of humanoid robots, thereby improving the balance stability of humanoid robots.

[0045] Please see Figure 2 This embodiment provides a robot's center of mass position calculation system, including a parameter acquisition module, an external force acquisition module, a control mode decision module, and a center of mass position calculation module, specifically: The parameter acquisition module is used to acquire the mass of several robotic arm links, the height of the center of mass of several robotic arm links relative to the ground, the coordinates of the center of mass of several robotic arm links in the original coordinate system, the angular velocity of several robotic arm links, the measured joint angle of the robotic arm, the measured joint angular velocity of the robotic arm, and the measured external force of the robotic arm joint. The external force acquisition module is used to obtain the first mechanical arm dynamic equation, the mechanical arm joint angle error and calculate the mechanical arm joint angular velocity error based on the mechanical arm link mass, the height of the mechanical arm link center of mass relative to the ground, the coordinates of the mechanical arm link center of mass in the original coordinate system, the mechanical arm link angular velocity, the measured mechanical arm joint angle and the measured mechanical arm joint angular velocity. The control mode decision module is used to execute position control mode operation and obtain position control mode torque if the measured external force of the robotic arm joint is less than or equal to a preset external force threshold. The centroid position calculation module is used to calculate the robot's centroid position based on the position control mode torque, the preset rotation matrix, the centroid coordinates of the robot arm link in the original coordinate system, and the mass of the robot arm link. The position control mode operation includes: Based on the joint angle error of the robotic arm, the joint angular velocity error of the robotic arm, and the first dynamic equation of the robotic arm, the state feedback compensation and the state observer compensation are determined. The position control mode torque is obtained based on state feedback compensation and state observer compensation.

[0046] This embodiment provides a robot centroid position calculation system. In practical applications, only an external force acquisition module is needed. By establishing a first robotic arm dynamic equation, the mathematical relationship between the robotic arm joint motion and the force is clarified, providing a theoretical model for control compensation calculation. Simultaneously, the robotic arm joint angle error and angular velocity error are calculated, quantifying the deviation between the actual and ideal states, providing a basis for error correction in position control mode. Next, a control mode decision module is used. When the measured external force on the robotic arm joint is less than or equal to a preset external force threshold, the position control mode operation is triggered, ensuring that the control mode is adapted to the actual force conditions of the robotic arm. Then, based on the robotic arm joint angle error, angular velocity error, and the first robotic arm dynamic equation, the position control mode torque is obtained. This position control mode torque ensures the motion accuracy and stability of the robotic arm in low-force scenarios. Finally, the center of mass position calculation module is used to obtain the current spatial posture of the robot arm by combining the position control mode torque with the preset rotation matrix. Based on the updated rotation matrix, the center of mass coordinates of the robot arm links in the original coordinate system are transformed. Then, the robot's center of mass position is obtained by combining the mass of the robot arm links to measure the robot's balance state. This avoids the risk of tipping over caused by the center of mass shift due to changes in the position of the robot arm during multi-contact balance control of the humanoid robot. It achieves real-time and accurate control of the center of mass position of the humanoid robot, thereby improving the balance stability of the humanoid robot.

[0047] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A method of calculating a center-of-mass position of a robot, characterized by, The method comprises the following steps: obtaining a plurality of mechanical arm link masses, a plurality of mechanical arm link center-of-mass heights, a plurality of mechanical arm link center-of-mass coordinates in a plurality of original coordinate systems, a plurality of mechanical arm link angular velocities, measured mechanical arm joint angles, measured mechanical arm joint angular velocities, and measured mechanical arm joint external forces; based on the mechanical arm link masses, the mechanical arm link center-of-mass heights, the mechanical arm link center-of-mass coordinates in the original coordinate systems, the mechanical arm link angular velocities, the measured mechanical arm joint angles, and the measured mechanical arm joint angular velocities, obtaining a first mechanical arm dynamics equation, a mechanical arm joint angle error, and a calculated mechanical arm joint angular velocity error; if the measured mechanical arm joint external force is less than or equal to a preset external force threshold, performing a position control mode operation to obtain a position control mode torque; based on the position control mode torque, a preset rotation matrix, the mechanical arm link center-of-mass coordinates in the original coordinate systems, and the mechanical arm link masses, calculating a robot center-of-mass position; wherein the position control mode operation comprises: based on the mechanical arm joint angle error, the mechanical arm joint angular velocity error, and the first mechanical arm dynamics equation, determining a state feedback compensation and a state observer compensation; based on the state feedback compensation and the state observer compensation, obtaining the position control mode torque.

2. The method of claim 1, wherein, The method further comprises the following steps: if the measured mechanical arm joint external force is greater than the preset external force threshold, performing an impedance control mode operation to obtain an impedance control mode torque; based on the impedance control mode torque, the preset rotation matrix, the mechanical arm link center-of-mass coordinates in the original coordinate systems, and the mechanical arm link masses, calculating the robot center-of-mass position; wherein the impedance control mode operation comprises: based on the mechanical arm joint angle error, the mechanical arm joint angular velocity error, a preset positive definite mechanical arm joint mass matrix, a preset positive definite damping matrix, a preset positive definite stiffness matrix, and the measured mechanical arm joint external force, establishing an impedance dynamics equation; based on the impedance dynamics equation and the first mechanical arm dynamics equation, obtaining the impedance control mode torque.

3. The method of claim 2, wherein, The method further comprises the following steps: based on the impedance control mode torque, the preset rotation matrix, the mechanical arm link center-of-mass coordinates in the original coordinate systems, and the mechanical arm link masses, calculating the robot center-of-mass position; comprises: based on the impedance control mode torque, obtaining a second mechanical arm joint angle, and updating the preset rotation matrix based on the second mechanical arm joint angle to obtain a second rotation matrix; based on the second rotation matrix and the mechanical arm link center-of-mass coordinates in the original coordinate systems, calculating second mechanical arm link center-of-mass coordinates relative to the ground; 4. The method of claim 1, wherein, based on the second mechanical arm link center-of-mass coordinates relative to the ground and the mechanical arm link masses, calculating the robot center-of-mass position. The method further comprises the following steps: based on the mechanical arm link masses, the mechanical arm link center-of-mass heights, the mechanical arm link center-of-mass coordinates in the original coordinate systems, the mechanical arm link angular velocities, the measured mechanical arm joint angles, and the measured mechanical arm joint angular velocities, obtaining a first mechanical arm dynamics equation, a mechanical arm joint angle error, and a calculated mechanical arm joint angular velocity error; comprises: based on the mechanical arm link masses, the mechanical arm link center-of-mass heights, the mechanical arm link center-of-mass coordinates in the original coordinate systems, and the mechanical arm link angular velocities, obtaining a mass matrix, a Coriolis centripetal force matrix, and a gravity vector to establish the first mechanical arm dynamics equation; Based on the measured mechanical arm joint angle and the preset target angle function, the mechanical arm joint angle error is calculated; Based on the measured mechanical arm joint angular velocity and the preset target angular velocity function, the mechanical arm joint angular velocity error is calculated.

5. The method of claim 4, wherein, The first mechanical arm dynamics equation is established based on the mass matrix, the Coriolis centripetal force matrix and the gravity vector obtained based on the mechanical arm link mass, the mechanical arm link mass center height, the mechanical arm link mass center coordinates in the original coordinate system and the mechanical arm link angular velocity. Based on the preset rotation matrix and the mechanical arm link mass center coordinates in the original coordinate system, the initial mechanical arm link mass center coordinates relative to the ground are calculated. Based on the initial mechanical arm link mass center coordinates relative to the ground, the mechanical arm link mass center linear velocity relative to the ground is obtained. Based on the mechanical arm link mass, the inertia tensor is obtained. Based on the mechanical arm link mass, the mechanical arm link mass center linear velocity relative to the ground, the mechanical arm link angular velocity and the inertia tensor, the total kinetic energy is obtained. Based on the mechanical arm link mass and the mechanical arm link mass center height, the total potential energy is obtained. Based on the total kinetic energy and the total potential energy, the mass matrix, the Coriolis centripetal force matrix and the gravity vector are obtained to establish the first mechanical arm dynamics equation.

6. The method of claim 4, wherein, The state feedback compensation and the state observer compensation are determined based on the mechanical arm joint angle error, the mechanical arm joint angular velocity error and the first mechanical arm dynamics equation; including: The state feedback compensation is obtained based on the preset eigenvalue matrix, the preset free matrix, the mechanical arm joint angle error, the mechanical arm joint angular velocity error and the first mechanical arm dynamics equation. The state observer compensation is determined based on the preset diagonal matrix, the mechanical arm joint angular velocity error, the gravity vector, the state feedback compensation and the preset disturbance observation auxiliary variable.

7. The method of claim 6, wherein, The state feedback compensation is obtained based on the preset eigenvalue matrix, the preset free matrix, the mechanical arm joint angle error, the mechanical arm joint angular velocity error and the first mechanical arm dynamics equation; including: Based on the preset eigenvalue matrix and the preset free matrix, the finite characteristic vector matrix and the parameter matrix are calculated. Based on the finite characteristic vector matrix and the parameter matrix, the feedback gain matrix is obtained. Based on the mechanical arm joint angle error and the mechanical arm joint angular velocity error, the state vector is constructed. Based on the preset unit matrix, the feedback gain matrix, the mass matrix and the Coriolis centripetal force matrix, the first state matrix and the input matrix are constructed. Based on the first state matrix, the state vector and the input matrix, the input vector is obtained. Based on the input vector, the feedback gain matrix, the mechanical arm joint angle error, the mechanical arm joint angular velocity error, the preset unit matrix and the gravity vector, the state feedback compensation is obtained.

8. The method of claim 6, wherein, The state observer compensation is determined based on the preset diagonal matrix, the mechanical arm joint angular velocity error, the gravity vector, the state feedback compensation and the preset disturbance observation auxiliary variable; including: Based on the preset diagonal matrix and the mechanical arm joint angular velocity error, the observer gain matrix is obtained. Based on the observer gain matrix, the gravity vector, the state feedback compensation and the preset disturbance observation auxiliary variable, the disturbance observation value is calculated to determine the state observer compensation.

9. The method of claim 1, wherein, The robot mass center position is calculated based on the position control mode torque, the preset rotation matrix, the mechanical arm link mass center coordinates in the original coordinate system and the mechanical arm link mass; including: Based on the position control mode torque, the first mechanical arm joint angle is acquired, and the first rotation matrix is updated based on the first mechanical arm joint angle, and the first rotation matrix is acquired; Based on the first rotation matrix and the mechanical arm link centroid coordinates in the original coordinate system, the first mechanical arm link centroid coordinates are calculated. Based on the first mechanical arm link centroid coordinates and the mechanical arm link mass, the robot centroid position is calculated.

10. A center-of-mass position calculation system of a robot characterized by comprising: It comprises a parameter acquisition module, an external force acquisition module, a control mode decision module and a centroid position calculation module, specifically: The parameter acquisition module is used to acquire a plurality of mechanical arm link masses, a plurality of mechanical arm link centroid heights, a plurality of mechanical arm link centroid coordinates in the original coordinate system, a plurality of mechanical arm link angular velocities, measured mechanical arm joint angles, measured mechanical arm joint angular velocities and measured mechanical arm joint external forces; The external force acquisition module is used to acquire the first mechanical arm dynamics equation, the mechanical arm joint angle error and the calculated mechanical arm joint angular velocity error based on the mechanical arm link mass, the mechanical arm link centroid height, the mechanical arm link centroid coordinates in the original coordinate system, the mechanical arm link angular velocity, the measured mechanical arm joint angle and the measured mechanical arm joint angular velocity; The control mode decision module is used to execute the position control mode operation if the measured mechanical arm joint external force is less than or equal to the preset external force threshold, and the position control mode torque is acquired; The centroid position calculation module is used to calculate the robot centroid position based on the position control mode torque, the preset rotation matrix, the mechanical arm link centroid coordinates in the original coordinate system and the mechanical arm link mass; The position control mode operation comprises: Based on the mechanical arm joint angle error, the mechanical arm joint angular velocity error and the first mechanical arm dynamics equation, the state feedback compensation and the state observer compensation are determined; Based on the state feedback compensation and the state observer compensation, the position control mode torque is acquired.

Images