Floating base robot mechanical arm stable trajectory planning method and system

Abstract

The present application is suitable for the field of robots, and particularly relates to a mechanical arm stable trajectory planning method and system of a floating base robot. The present application establishes a kinematics model of a mechanical arm; obtains a starting pose of each joint of the mechanical arm according to the kinematics model of the mechanical arm, and sets a target point pose; performs inverse kinematics iteration solving through an inverse kinematics solver according to the starting pose and the target point pose, and calculates a target joint configuration; performs calculation through a linear interpolation method according to the starting pose and the target joint configuration, and obtains a motion trajectory of the mechanical arm; calculates a mechanical arm centroid position of each interpolation point in the motion trajectory, and obtains a mechanical arm centroid coordinate corresponding to each interpolation point; and performs iteration optimization through a preset method according to the mechanical arm centroid coordinate, and obtains an optimal motion trajectory. The motion trajectory of the floating base robot is effectively controlled, the centroid change is maximally reduced, and the operation efficiency and stability of the robot on a power transmission line are improved.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a method and system for planning the stable trajectory of a robotic arm of a floating base robot. Background Technology

[0002] Floating-base dual-arm robots operating in the unique environment of high-voltage power transmission lines can perform a variety of tasks such as inspection, dismantling, and maintenance. These robots typically consist of a base placed on the power line and a series of robotic arms composed of one or more links and joints, with a dynamic coupling between them. However, in a floating-base environment, the movement of the robotic arms can affect the position and orientation of the base, thus influencing the overall center of mass of the robot and consequently the accuracy and stability of the robotic arm movements.

[0003] To minimize the impact of robotic arm motion on the robot base, reasonable trajectory planning is necessary to reduce the amplitude of base posture changes while satisfying motion constraints. Currently, several methods have been proposed to adjust the robot's center of mass to maintain stability. For example, based on sensor information from each joint of the robotic arm, the weight of the load and the torque of the robotic arm can be acquired in real time; the end effector load can be mapped to the robot's center of mass, and full-body coordinated stability control can be implemented. Another method involves adjusting the center of mass by controlling the position of the counterweight. By fixing the counterweight to the upper part of the base, the center of gravity is lowered, improving the stability of the robotic arm. Ball bearings and spring cylinders are installed inside the rotating shaft to ensure the smoothness and compactness of the robotic arm's rotation. In practical applications, since the motion of the robotic arm can significantly affect the position and orientation of the base's center of mass, studying the stability of its motion trajectory is particularly important. Especially when the robot is performing tasks online, changes in the center of mass directly affect its stability and motion accuracy.

[0004] However, existing motor output torque balancing robots have the following drawbacks: the actual output torque of the motor may be too large or too small, and the data acquired by the sensors may not be accurate enough; when handling dynamically changing heavy objects or environments, the system may face challenges in response speed and adaptability. Therefore, the applicability of this method may be limited. Adjusting the robot's center of gravity position with counterweights has the following disadvantages: it can guarantee that the robot will eventually reach a stable state, but it cannot guarantee that the robot will remain stable throughout its movement, i.e., it cannot guarantee the dynamic stability of the robot's center of gravity; it increases the robot's complexity and cost. In the design and manufacturing process of the robot, the position and control mechanism of the counterweights need to be additionally considered and integrated, which increases the manufacturing and maintenance costs of the robot; the addition of counterweights increases the overall weight and volume of the robot, reducing its mobility and efficiency.

[0005] Therefore, there is an urgent need for a new method and system for stabilizing the trajectory of the robotic arm of a floating base robot to solve the above problems. Summary of the Invention

[0006] This invention provides a method and system for stabilizing the trajectory of a floating-base robot arm, aiming to effectively control the motion trajectory of a floating-base dual-arm robot and minimize changes in the center of mass.

[0007] In a first aspect, the present invention proposes a method for planning the stable trajectory of a robotic arm of a floating base robot, the method comprising the following steps:

[0008] S1. Establish the kinematic model of the robotic arm;

[0009] S2. Obtain the initial pose of each joint of the robotic arm according to the kinematic model of the robotic arm, and set the target point pose;

[0010] S3. Based on the initial pose and the target point pose, perform inverse kinematics iterative solution using an inverse kinematics solver to calculate the target joint configuration.

[0011] S4. The motion trajectory of the robotic arm is obtained by linear interpolation based on the initial pose and the target joint configuration.

[0012] S5. Calculate the position of the robot arm's center of mass at each interpolation point in the motion trajectory to obtain the coordinates of the robot arm's center of mass corresponding to each interpolation point;

[0013] S6. Based on the coordinates of the robot arm's center of mass, perform iterative optimization using a preset method to obtain the optimal motion trajectory.

[0014] Preferably, in step S1, the kinematic model of the robotic arm is established using the improved DH method based on the link twist angle, link length, link distance, and joint angle in the robotic arm.

[0015] Preferably, step S3 includes the following sub-steps:

[0016] S31. Calculate the current end effector pose of the robotic arm based on the current joint configuration of the robotic arm;

[0017] S32. Calculate the error between the current end pose and the target pose, and determine whether the error is less than a preset threshold. If yes, stop the iteration and output the current joint configuration as the target joint configuration; otherwise, proceed to step S33.

[0018] S33. Calculate the Jacobian matrix of the current end joint of the robotic arm, and calculate the joint velocity of the robotic arm based on the Jacobian matrix;

[0019] S34. Update the current joint configuration based on the joint speed using a preset integral.

[0020] Preferably, step S6 further includes the following sub-steps:

[0021] S61. Calculate the average value of the change in the center of mass of each robotic arm during the movement based on the coordinates of the center of mass of each robotic arm, and obtain the first average value of the change in the center of mass;

[0022] S62. Randomly generate a first path point and a second path point, calculate the motion trajectory of the robotic arm from the starting pose through the first path point, the second path point and the target point pose, and the average change of the center of mass of the robotic arm during the motion, to obtain the random motion trajectory and the average change of the second center of mass.

[0023] S63. Determine whether the average value of the first centroid change is greater than the average value of the second centroid change. If yes, update the average value of the second centroid change as the average value of the first centroid change and proceed to step S64; otherwise, return to step S62.

[0024] S64. Determine whether the current iteration number exceeds the preset number. If yes, output the current random motion trajectory as the optimal motion trajectory; otherwise, return to step S62.

[0025] Secondly, the present invention also provides a stabilization trajectory planning system for the robotic arm of a floating base robot, comprising:

[0026] The model building module is used to build the kinematic model of the robotic arm;

[0027] The pose module is used to obtain the initial pose of each joint of the robotic arm according to the kinematic model of the robotic arm, and to set the target point pose.

[0028] The joint configuration module is used to calculate the target joint configuration by performing inverse kinematics iterative solution using an inverse kinematics solver based on the initial pose and the target point pose.

[0029] The motion trajectory module is used to calculate the motion trajectory of the robotic arm using linear interpolation based on the initial pose and the target joint configuration.

[0030] The centroid coordinate module is used to calculate the position of the manipulator's centroid at each interpolation point in the motion trajectory, and obtain the centroid coordinates of the manipulator corresponding to each interpolation point.

[0031] The trajectory optimization module is used to iteratively optimize the motion trajectory based on the coordinates of the robot arm's center of mass using a preset method.

[0032] Preferably, in the model building module, the kinematic model of the robotic arm is built using an improved DH method based on the link twist angle, link length, link distance, and joint angle in the robotic arm.

[0033] Preferably, the joint configuration module includes the following sub-modules:

[0034] An end-effector pose module is used to calculate the current end-effector pose of the robotic arm based on the current joint configuration of the robotic arm.

[0035] The error calculation module is used to calculate the error between the current end pose and the target pose, and determine whether the error is less than a preset threshold. If so, the iteration stops and the current joint configuration is output as the target joint configuration; otherwise, the module proceeds to the joint velocity calculation module.

[0036] The joint speed calculation module is used to calculate the Jacobian matrix of the current end joint of the robotic arm, and to calculate the joint speed of the robotic arm based on the Jacobian matrix.

[0037] An update module is used to update the current joint configuration based on the joint speed using a preset integral.

[0038] Preferably, the trajectory optimization module further includes the following sub-modules:

[0039] The first calculation module is used to calculate the average value of the change in the center of mass of the robotic arm during the movement process based on the coordinates of the center of mass of each robotic arm, and to obtain the first average value of the change in the center of mass.

[0040] The second calculation module is used to randomly generate a first path point and a second path point, calculate the motion trajectory of the robotic arm from the starting pose through the first path point, the second path point and the target point pose, and the average change of the center of mass of the robotic arm during the motion, and obtain the random motion trajectory and the average change of the second center of mass.

[0041] The average value update module is used to determine whether the average value of the first centroid change is greater than the average value of the second centroid change. If so, the average value of the second centroid change is used as the average value of the first centroid change for updating, and the module is then entered into the output module. If not, the module returns to the second calculation module.

[0042] The output module is used to determine whether the current iteration number exceeds the preset number. If so, the current random motion trajectory is output as the optimal motion trajectory; otherwise, the process returns to the second calculation module.

[0043] Compared with existing technologies, this invention establishes a kinematic model of the robotic arm; obtains the initial pose of each joint of the robotic arm based on the kinematic model, and sets the target point pose; performs inverse kinematics iterative solution using an inverse kinematics solver based on the initial pose and target point pose to calculate the target joint configuration; calculates the robotic arm's motion trajectory using linear interpolation based on the initial pose and target joint configuration; calculates the position of the robotic arm's center of mass at each interpolation point in the motion trajectory to obtain the corresponding coordinates of the robotic arm's center of mass; and iteratively optimizes the motion trajectory using a preset method based on the coordinates of the robotic arm's center of mass to obtain the optimal motion trajectory. This effectively controls the motion trajectory of the floating-base dual-arm robot, minimizes the change in the center of mass, and improves the operating efficiency and stability of the dual-arm robot on power transmission lines. Attached Figure Description

[0044] The present invention will now be described in detail with reference to the accompanying drawings. The above and other aspects of the present invention will become clearer and more readily understood through the detailed description following the accompanying drawings. In the drawings:

[0045] Figure 1 This is a flowchart of the robotic arm stabilization trajectory planning method for a floating base robot provided in an embodiment of the present invention;

[0046] Figure 2 This is a schematic diagram of the robotic arm structure of the robotic arm stabilization trajectory planning method for the floating base robot provided in an embodiment of the present invention;

[0047] Figure 3 This is a schematic diagram of the change curve of the center of mass of the robotic arm in a method for planning the stable trajectory of the robotic arm of a floating base robot provided in an embodiment of the present invention.

[0048] Figure 4 This is a schematic diagram of a stable trajectory planning system for the robotic arm of a floating base robot provided in an embodiment of the present invention. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0050] Please refer to Figures 1-3 In a first aspect, the present invention proposes a method for planning the stable trajectory of a robotic arm of a floating base robot, the method comprising the following steps:

[0051] S1. Establish the kinematic model of the robotic arm.

[0052] In this embodiment of the invention, in step S1, the kinematic model of the robotic arm is established using the improved DH method based on the link twist angle, link length, link distance, and joint angle in the robotic arm.

[0053] Specifically, a simulation environment for a floating-base dual-arm robot was established. The robot model consists of a base placed on a power transmission line and two six-axis robotic arms. The power transmission line has a diameter of 10mm. Figure 2 As shown, Figure 2 This is a schematic diagram of the robotic arm structure for the stable trajectory planning method of the floating base robot provided in this embodiment of the invention. As shown in Table 1, the kinematic model of a single robotic arm is established using the improved DH method, and the corresponding kinematic model of the two arms is obtained based on the kinematic model of the single robotic arm and the constraint relationship between the two arms. The URDF model of the two-arm robot is imported into the simulator, and the robot joint control mode is set to realize that the joints at the connection between the base of the two-arm robot and the power transmission line are affected by the movement of the joints of the two arms. When the joints of the two arms move, the two-arm robot rotates around the line under the influence of gravity and the dynamic coupling relationship between the joint links, thereby dynamically simulating the real online environment.

[0054] Table 1: Robotic Arm DH Parameters

[0055]

[0056] Please refer to Table 1. The improved DH method uses four parameters to define the transformation of a coordinate system to adjacent coordinate systems. These parameters are derived from two link parameters: link twist angle. Link length a i-1 Two joint parameters: link distance d i (mm), joint angle θ i composition.

[0057] S2. Obtain the initial pose of each joint of the robotic arm according to the kinematic model of the robotic arm, and set the target point pose.

[0058] In this embodiment of the invention, an inverse kinematics solver is used to perform inverse kinematics solutions on both arms of the robot. The inverse kinematics solver calculates the joint configuration q, i.e., the angles of each joint, that enables the robotic arm to reach the target point based on the robot's geometry and constraints.

[0059] S3. Based on the initial pose and the target point pose, perform inverse kinematics iterative solution using an inverse kinematics solver to calculate the target joint configuration.

[0060] In this embodiment of the invention, step S3 includes the following sub-steps:

[0061] S31. Calculate the current end effector pose of the robotic arm based on the current joint configuration of the robotic arm;

[0062] S32. Calculate the error between the current end pose and the target pose, and determine whether the error is less than a preset threshold. If yes, stop the iteration and output the current joint configuration as the target joint configuration; otherwise, proceed to step S33.

[0063] S33. Calculate the Jacobian matrix of the current end joint of the robotic arm, and calculate the joint velocity of the robotic arm based on the Jacobian matrix;

[0064] The Jacobian matrix represents the relationship between the velocities of each joint of the robotic arm and the end effector velocity. This is expressed by the formula... Calculate the Jacobian matrix, where J(q) is the Jacobian matrix of the robotic arm and x is the pose vector of the end effector.

[0065] Joint velocity v is expressed by the formula v = -J T (JJ T +damp·I) -1 ·e is obtained, where J is the Jacobian matrix, e is the error vector between the current pose and the desired pose of the end effector, damp is the set damping coefficient, and I is the six-dimensional identity matrix.

[0066] S34. Update the current joint configuration based on the joint speed using a preset integral.

[0067] S4. The motion trajectory of the robotic arm is obtained by calculating using linear interpolation based on the initial pose and the target joint configuration.

[0068] In this embodiment of the invention, after obtaining the initial pose (initial joint configuration) and target joint configuration of the robotic arm, linear interpolation is used to obtain the trajectory of the robotic arm. By uniformly inserting a series of intermediate configurations between the initial and target joint configurations, a continuous motion trajectory of the robotic arm is generated, controlling the robotic arm to reach the target position.

[0069] S5. Calculate the position of the robot arm's center of mass at each interpolation point in the motion trajectory to obtain the coordinates of the robot arm's center of mass corresponding to each interpolation point.

[0070] In this embodiment of the invention, the mass of each link of the robot is first obtained, and the coordinates of the center of mass of each link are obtained. The overall coordinates of the robot's center of mass are then calculated using a weighted average. At each interpolation point, the robot arm pose is updated, and the robot's center of mass coordinates at that time are obtained, resulting in a curve showing the change in the robot's center of mass. Please refer to... Figure 3 , Figure 3This is a schematic diagram of the centroid change curve of a floating base robot's robotic arm in a stable trajectory planning method provided by an embodiment of the present invention. The horizontal axis represents the number of interpolations, and the vertical axis represents the coordinate changes on the X, Y, and Z axes, respectively. The horizontal axes 1-100 and 101-200 represent the centroid change curves of the right and left arms of the robotic arm as they reach the target position, respectively.

[0071] S6. Based on the coordinates of the robot arm's center of mass, perform iterative optimization using a preset method to obtain the optimal motion trajectory.

[0072] In this embodiment of the invention, to find the trajectory with the smallest change in the center of mass, two path points are first randomly generated. Then, a trajectory is generated through inverse kinematics, and the average change in the center of mass is calculated. During the iteration process, the optimal trajectory is continuously compared and updated until the upper limit of the number of iterations or the continuous improvement threshold is reached. The final trajectory obtained is the current optimal trajectory of the robotic arm.

[0073] In this embodiment of the invention, step S6 further includes the following sub-steps:

[0074] S61. Calculate the average value of the change in the center of mass of each robotic arm during the movement based on the coordinates of the center of mass of each robotic arm, and obtain the first average value of the change in the center of mass;

[0075] S62. Randomly generate a first path point and a second path point, calculate the motion trajectory of the robotic arm from the starting pose through the first path point, the second path point and the target point pose, and the average change of the center of mass of the robotic arm during the motion, to obtain the random motion trajectory and the average change of the second center of mass.

[0076] S63. Determine whether the average value of the first centroid change is greater than the average value of the second centroid change. If yes, update the average value of the second centroid change as the average value of the first centroid change and proceed to step S64; otherwise, return to step S62.

[0077] S64. Determine whether the current iteration number exceeds the preset number. If yes, output the current random motion trajectory as the optimal motion trajectory; otherwise, return to step S62.

[0078] Compared with existing technologies, this invention establishes a kinematic model of the robotic arm; obtains the initial pose of each joint of the robotic arm based on the kinematic model, and sets the target point pose; performs inverse kinematics iterative solution using an inverse kinematics solver based on the initial pose and target point pose to calculate the target joint configuration; calculates the robotic arm's motion trajectory using linear interpolation based on the initial pose and target joint configuration; calculates the position of the robotic arm's center of mass at each interpolation point in the motion trajectory to obtain the corresponding coordinates of the robotic arm's center of mass; and iteratively optimizes the motion trajectory using a preset method based on the coordinates of the robotic arm's center of mass to obtain the optimal motion trajectory. This effectively controls the motion trajectory of the floating-base dual-arm robot, minimizes the change in the center of mass, and improves the operating efficiency and stability of the dual-arm robot on power transmission lines.

[0079] Please refer to Figure 4 Secondly, the present invention also provides a robotic arm stabilization trajectory planning system 200 for a floating base robot, comprising:

[0080] 201. Model building module, used to build the kinematic model of the robotic arm.

[0081] In this embodiment of the invention, the model building module establishes the kinematic model of the robotic arm by means of the improved DH method based on the link twist angle, link length, link distance and joint angle in the robotic arm.

[0082] 202. Pose module, used to obtain the initial pose of each joint of the robotic arm according to the kinematic model of the robotic arm, and set the target point pose.

[0083] 203. Joint configuration module, used to calculate the target joint configuration by performing inverse kinematics iterative solution using an inverse kinematics solver based on the initial pose and the target point pose.

[0084] In this embodiment of the invention, the joint configuration module includes the following sub-modules:

[0085] An end-effector pose module is used to calculate the current end-effector pose of the robotic arm based on the current joint configuration of the robotic arm.

[0086] The error calculation module is used to calculate the error between the current end pose and the target pose, and determine whether the error is less than a preset threshold. If so, the iteration stops and the current joint configuration is output as the target joint configuration; otherwise, the module proceeds to the joint velocity calculation module.

[0087] The joint speed calculation module is used to calculate the Jacobian matrix of the current end joint of the robotic arm, and to calculate the joint speed of the robotic arm based on the Jacobian matrix.

[0088] An update module is used to update the current joint configuration based on the joint speed using a preset integral.

[0089] 204. Motion trajectory module, used to calculate the motion trajectory of the robotic arm by linear interpolation based on the initial pose and the target joint configuration.

[0090] 205. Centroid coordinate module, used to calculate the position of the manipulator's centroid at each interpolation point in the motion trajectory, and obtain the centroid coordinates of the manipulator corresponding to each interpolation point.

[0091] 206. The trajectory optimization module is used to perform iterative optimization based on the coordinates of the robot arm's center of mass using a preset method to obtain the optimal motion trajectory.

[0092] In this embodiment of the invention, the trajectory optimization module further includes the following sub-modules:

[0093] The first calculation module is used to calculate the average value of the change in the center of mass of the robotic arm during the movement process based on the coordinates of the center of mass of each robotic arm, and to obtain the first average value of the change in the center of mass.

[0094] The second calculation module is used to randomly generate a first path point and a second path point, calculate the motion trajectory of the robotic arm from the starting pose through the first path point, the second path point and the target point pose, and the average change of the center of mass of the robotic arm during the motion, and obtain the random motion trajectory and the average change of the second center of mass.

[0095] The average value update module is used to determine whether the average value of the first centroid change is greater than the average value of the second centroid change. If so, the average value of the second centroid change is used as the average value of the first centroid change for updating, and the module is then entered into the output module. If not, the module returns to the second calculation module.

[0096] The output module is used to determine whether the current iteration number exceeds the preset number. If so, the current random motion trajectory is output as the optimal motion trajectory; otherwise, the process returns to the second calculation module.

[0097] The robotic arm stabilization trajectory planning system 200 of the floating base robot can implement the steps in the method of the above embodiments and achieve the same technical effect. Refer to the description in the above embodiments, which will not be repeated here.

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

[0099] The embodiments of the present invention have been described above with reference to the accompanying drawings. The disclosed embodiments are merely preferred embodiments of the present invention. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many equivalent changes in form without departing from the spirit and scope of the claims of the present invention, and all such changes are within the protection scope of the present invention.

Claims

1. A method for planning the stable trajectory of a robotic arm of a floating-base robot, characterized in that, The robotic arm stable trajectory planning method includes the following steps: S1. Establish the kinematic model of the robotic arm; S2. Obtain the initial pose of each joint of the robotic arm according to the kinematic model of the robotic arm, and set the target point pose; S3. Based on the initial pose and the target point pose, perform inverse kinematics iterative solution using an inverse kinematics solver to calculate the target joint configuration. S4. The motion trajectory of the robotic arm is obtained by linear interpolation based on the initial pose and the target joint configuration. S5. Calculate the position of the robot arm's center of mass at each interpolation point in the motion trajectory to obtain the coordinates of the robot arm's center of mass corresponding to each interpolation point; S6. Based on the coordinates of the robot arm's center of mass, perform iterative optimization using a preset method to obtain the optimal motion trajectory; Step S6 also includes the following sub-steps: S61. Calculate the average value of the change in the center of mass of each robotic arm during the movement based on the coordinates of the center of mass of each robotic arm, and obtain the first average value of the change in the center of mass; S62. Randomly generate a first path point and a second path point, calculate the motion trajectory of the robotic arm from the starting pose through the first path point, the second path point and the target point pose, and the average change of the center of mass of the robotic arm during the motion, to obtain the random motion trajectory and the average change of the second center of mass. S63. Determine whether the average value of the first centroid change is greater than the average value of the second centroid change. If yes, update the average value of the second centroid change as the average value of the first centroid change and proceed to step S64; otherwise, return to step S62. S64. Determine whether the current iteration number exceeds the preset number. If yes, output the current random motion trajectory as the optimal motion trajectory; otherwise, return to step S62.

2. The method for stabilizing the trajectory of the robotic arm of a floating base robot as described in claim 1, characterized in that, In step S1, the kinematic model of the robotic arm is established using the improved DH method based on the link twist angle, link length, link distance, and joint angle in the robotic arm.

3. The method for planning the stable trajectory of the robotic arm of a floating base robot as described in claim 1, characterized in that, Step S3 includes the following sub-steps: S31. Calculate the current end effector pose of the robotic arm based on the current joint configuration of the robotic arm; S32. Calculate the error between the current end pose and the target pose, determine whether the error is less than a preset threshold, and if so, stop the iteration and output the current joint configuration as the target joint configuration. If not, proceed to step S33; S33. Calculate the Jacobian matrix of the current end joint of the robotic arm, and calculate the joint velocity of the robotic arm based on the Jacobian matrix; S34. Update the current joint configuration based on the joint speed using a preset integral.

4. A system for stabilizing the trajectory of a robotic arm of a floating-base robot, characterized in that, include: The model building module is used to build the kinematic model of the robotic arm; The pose module is used to obtain the initial pose of each joint of the robotic arm according to the kinematic model of the robotic arm, and to set the target point pose. The joint configuration module is used to calculate the target joint configuration by performing inverse kinematics iterative solution using an inverse kinematics solver based on the initial pose and the target point pose. The motion trajectory module is used to calculate the motion trajectory of the robotic arm using linear interpolation based on the initial pose and the target joint configuration. The centroid coordinate module is used to calculate the position of the manipulator's centroid at each interpolation point in the motion trajectory, and obtain the centroid coordinates of the manipulator corresponding to each interpolation point. The trajectory optimization module is used to iteratively optimize the motion trajectory based on the coordinates of the robot arm's center of mass using a preset method.

5. The robotic arm stabilization trajectory planning system for a floating base robot as described in claim 4, characterized in that, In the model building module, the kinematic model of the robotic arm is established using the improved DH method based on the link twist angle, link length, link distance, and joint angle in the robotic arm.

6. The robotic arm stabilization trajectory planning system for a floating base robot as described in claim 4, characterized in that, The joint configuration module includes the following sub-modules: An end-effector pose module is used to calculate the current end-effector pose of the robotic arm based on the current joint configuration of the robotic arm. The error calculation module is used to calculate the error between the current end pose and the target pose, and determine whether the error is less than a preset threshold. If so, the iteration stops and the current joint configuration is output as the target joint configuration; otherwise, the module proceeds to the joint velocity calculation module. The joint speed calculation module is used to calculate the Jacobian matrix of the current end joint of the robotic arm, and to calculate the joint speed of the robotic arm based on the Jacobian matrix. An update module is used to update the current joint configuration based on the joint speed using a preset integral.

7. The robotic arm stabilization trajectory planning system for a floating base robot as described in claim 4, characterized in that, The trajectory optimization module also includes the following sub-modules: The first calculation module is used to calculate the average value of the change in the center of mass of the robotic arm during the movement process based on the coordinates of the center of mass of each robotic arm, and to obtain the first average value of the change in the center of mass. The second calculation module is used to randomly generate a first path point and a second path point, calculate the motion trajectory of the robotic arm from the starting pose through the first path point, the second path point and the target point pose, and the average change of the center of mass of the robotic arm during the motion, and obtain the random motion trajectory and the average change of the second center of mass. The average value update module is used to determine whether the average value of the first centroid change is greater than the average value of the second centroid change. If so, the average value of the second centroid change is used as the average value of the first centroid change for updating, and the module is then entered into the output module. If not, the module returns to the second calculation module. The output module is used to determine whether the current iteration number exceeds the preset number. If so, the current random motion trajectory is output as the optimal motion trajectory; otherwise, the process returns to the second calculation module.

Images