Method and system for controlling motion of heavy-load mechanical arm

Abstract

The invention relates to the technical field of old-age nursing robots, and provides a heavy-load mechanical arm motion control method and system.The method comprises the steps that a mechanical arm control host receives environment information collected by a multi-mode sensor in real time, the current state of a robot and natural language task description, and a sensing state set is formed; the mechanical arm control host adopts an ROS2-based distributed communication architecture to publish the sensing state set through an ROS2 topic; the mechanical arm control slave receives the released ROS2 topic and outputs a task-related action instruction to the mechanical arm control host through a VLA model; and after receiving the action instruction, the control node maps the action instruction into the control quantity of the joint space of the mechanical arm and sends the control quantity to the servo driver, so that the actual movement of the mechanical arm is realized. Through the method, the combination of the high-performance execution capability of the heavy-load mechanical arm and the advanced VLA model reasoning capability is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of elderly care robots, and particularly relates to a method and system for motion control of a large-load mechanical arm. BACKGROUND

[0002] With the intensification of population aging, the demand for elderly care is growing year by year. Existing mechanical arms are mainly lightweight mechanical arms or service robots, which can complete basic actions such as dish serving and object delivery, but in actual elderly care scenarios, tasks such as assisting the elderly to turn over, pouring water, and feeding require higher requirements for robots. On the one hand, robots need to have high carrying capacity to realize large-load care operations; on the other hand, robots need to have complex environment perception and task reasoning capabilities to generate reasonable action sequences according to natural language instructions.

[0003] However, existing large-load mechanical arms mostly use traditional trajectory planning or teaching and reproduction methods, which lack flexibility and intelligence and cannot adapt to changing care scenarios. For example, the Chinese patent application for invention with publication number CN118650612A, “Motion control optimization method of multi-constrained large-load industrial robot”, discloses a motion control optimization method of a multi-constrained large-load industrial robot. The method establishes a mechanical arm kinematics model and a mechanical arm dynamics model according to the joint parameters and motion restrictions of the mechanical arm; imports an obstacle model and generates a planning path; optimizes the generated planning path according to the kinematics model and the dynamics model of the mechanical arm to obtain an optimized trajectory; defines angle interpolation smoothness; establishes a multi-objective trajectory optimization model, and evaluates the optimized trajectory through angle interpolation smoothness and impact size; and iteratively optimizes to obtain an optimal path.

[0004] At the same time, although existing agent models can realize language-to-action mapping, they lack adaptability on large-load robots and are difficult to ensure the precision and safety of motion control. For example, the Chinese patent application for invention with publication number CN120170721A, “Flexible robot motion control system and method based on visual language action model”, discloses a flexible robot motion control system and method based on a visual language action model. The motion control module based on the VLA framework is installed inside the flexible robot and is used to control the flexible robot to perform actions based on the visual language action model and according to multi-joint perception information and voice instructions. Through intelligent control system design, the system complexity is significantly reduced, the control precision and flexibility are optimized, the coordination control problem of complex body units is solved, high-performance autonomous motion control is realized, the system cost is reduced, and the adaptability of the robot in a diversified environment is enhanced, enabling the robot to be widely deployed in complex application scenarios.

[0005] Therefore, how to combine the high-performance execution capability of the large-load mechanical arm with the advanced VLA model inference capability becomes a key problem in the research of the elderly care robot. SUMMARY

[0006] The technical problem to be solved by the present application is how to make up for the deficiencies of the prior art, combine the high-performance execution capability of the large-load mechanical arm with the advanced VLA model inference capability.

[0007] The present application solves the above technical problems by the following technical means: The present application provides a method for motion control of a large-load mechanical arm, comprising the following steps: S1, the mechanical arm control host receives environment information collected in real time by a multi-modal sensor, a current state of the robot and a natural language task description, to form a perception state set; S2, the mechanical arm control host publishes the perception state set through a ROS2 topic by using a distributed communication architecture based on ROS2; S3, a mechanical arm control slave responsible for VLA model inference receives the published ROS2 topic, and outputs a task-related action instruction to the mechanical arm control host through the VLA model; S4, after the control node receives the action instruction, it maps it into a control amount in the joint space of the mechanical arm; S5, the control layer sends the control amount in the joint space of the mechanical arm to a servo driver, to realize the actual motion of the mechanical arm, and updates the control gain through real-time torque feedback.

[0008] Further, the multi-modal sensor comprises a color camera, a depth camera, a six-dimensional torque sensor and a joint encoder.

[0009] Further, step S1 comprises the following steps: S11, color images are obtained by the color camera and the depth camera ; S12, a joint angle vector is fed back in real time by the joint encoder ; S13, torque feedback is obtained by the six-dimensional torque sensor ; S14, combined with a natural language task description , a perception state set is formed .

[0010] Further, the step S2 is specifically: The system uses C++ running on Ubuntu 22.04 and subscribes to camera and encoder topics via the ROS2 framework, including image streams, joint angle vectors, and force feedback signals. The rclcpp::Publisher mechanism is used to publish data streams to the / vla_observation topic at a message frequency of 5Hz. Image messages are compressed and encoded into sensor_msgs / Image type, and status messages are encapsulated into Float64MultiArray type to ensure real-time transmission and data synchronization.

[0011] Further, step S3 includes the following steps: The inputs to the S31 and VLA models are Visual-language encoder via VLA model Then, the joint embedding vector is obtained.

[0012] S32, VLA model action decoder embed vector Mapped to continuous action instructions As shown in the following formula

[0013] in, For the translation increment of the end effector; This represents the increment of the end attitude angle; This represents the opening and closing state of the grippers, where 1 indicates closed and 0 indicates open.

[0014] Further, step S4 includes the following steps: S41. Based on the current joint angle state, the control node obtains the current end-effector pose matrix through the positive kinematic function FK. As shown in the following formula:

[0015] S42, Control node extracts translation increment and in the current end pose matrix Construct the target end pose matrix based on Translation part ; S43, Control node extracts attitude angle increment and in the current end pose matrix Construct the target end pose matrix based on Rotating part ; S44, Control node based on target end pose With the current joint angle vector As the initial value, a numerical iterative inverse kinematics algorithm IK is called to solve the target joint angle vector As follows:

[0016] S45, using a numerical iterative method based on the minimum end position error, to improve the stability and continuity of the inverse kinematics algorithm solution, as follows:

[0017]

[0018] wherein, is the current joint angle increment vector, is the generalized inverse of the Jacobian matrix, and respectively represent the current and target end position, Δt is the control period.

[0019] Further, step S42 constructs the translation part of the target end position matrix Specifically: The current end position matrix is as follows:

[0020] wherein, is a 4x4 homogeneous transformation matrix, is a 3x3 current rotation matrix; represents the position vector of the current end effector in the base coordinate system; then,

[0021] That is, the translation part of the target end position matrix is obtained, as follows: .

[0022] Further, step S43 constructs the rotation part of the target end position matrix Specifically: First, the current Euler angle is solved from the current rotation matrix , and the pose angle increment is added to obtain the target Euler angle, as follows:

[0023]

[0024] ​​

[0025] Then, the target Euler angles are converted into the target rotation matrix using the Euler angle transformation rotation matrix function. Obtain the target end pose matrix The rotating part is as follows: .

[0026] Further, step S5 includes the following steps: S51, Target joint angle The data is sent to the servo driver to realize the actual movement of the robotic arm; S52, Real-time monitoring of position deviation With torque feedback As shown in the following formula:

[0027]

[0028] in, It is the 6-dimensional vector of the target's end pose. It is the 6-dimensional vector of the current end pose. For the first t Real-time torque feedback from the robotic arm. The torque signal is acquired by the torque sensor; S53. Update the control gain based on the feedback data, as follows:

[0029]

[0030] in, For the first t The proportional controller gain at time +1 For the first t The proportional controller gain at time 10:00 This is the proportional gain adjustment coefficient. For the first t Differential controller gain at time +1 For the first t The differential controller gain at time t, This is the differential gain adjustment coefficient.

[0031] This invention also provides a system for controlling the motion of a heavy-duty robotic arm. The system executes the above-described method during operation and includes the following modules: The status and task acquisition module is used by the robotic arm control host to receive environmental information, the current state of the robot, and natural language task descriptions collected in real time by multimodal sensors, forming a perception state set; The topic subscription module is used by the robotic arm control host to publish the sensed state set through ROS2 topics using a ROS2-based distributed communication architecture. The action instruction module is used by the robotic arm control slave responsible for VLA model inference to receive published ROS2 topics and output task-related action instructions to the robotic arm control host through the VLA model. The control quantity mapping module is used to map the motion commands received by the control node into control quantities in the joint space of the robotic arm. The control gain adjustment module is used by the control layer to send the control quantity of the robotic arm joint space to the servo driver to realize the actual movement of the robotic arm, and to update the control gain through real-time torque feedback.

[0032] The advantages of this invention are: (1) This invention achieves end-to-end mapping from natural language commands to precise physical actions by deeply integrating the vision-language-action (VLA) model with traditional kinematic control, which greatly improves the robot's autonomous decision-making ability. In terms of control strategy, it innovatively adopts a hybrid motion control architecture, which optimizes translation and rotation components separately, effectively solving the industry problem of inaccurate rotational actions under heavy load conditions.

[0033] (2) The robotic arm control system of the present invention has high precision execution capability, with an end-effector position error of less than 1.2 mm and an attitude error of less than 0.5°. It can remain stable and vibration-free even under a maximum load of 30 kg. At the same time, through the adaptive gain adjustment mechanism of torque feedback, smooth human-machine interaction is achieved, which significantly improves the safety of nursing operations.

[0034] (3) The distributed communication architecture based on ROS2 ensures the real-time performance and reliability of the system. The closed-loop control system enables the robot to adapt to diverse nursing scenarios such as "pouring water", "feeding", and "helping". While ensuring high load-bearing capacity, it significantly improves the intelligence level and human-machine collaboration ability of the elderly care robot, providing an innovative technical solution for solving the elderly care problem caused by population aging. Attached Figure Description

[0035] Figure 1 This is a flowchart illustrating a method for motion control of a heavy-load robotic arm according to an embodiment of the present invention. Figure 2 This is a schematic diagram of a scenario in which a robotic arm performs a "pouring water" task on an elderly care robot platform, according to an embodiment of the present invention. Detailed Implementation

[0036] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0037] Example 1 This embodiment provides a method for motion control of a large-load robotic arm, such as... Figure 2 As shown, this method is illustrated using a 14-DOF dual-arm, high-load elderly care robot as a platform and "pouring water" as the target task; the specific implementation process is as follows. Figure 1 As shown, it includes the following steps: S1. The robotic arm control unit receives environmental information, the current state of the robot, and a natural language task description collected in real time by multimodal sensors, forming a perception state set; the multimodal sensors include a color camera, a depth camera, a six-dimensional torque sensor, and a joint encoder. The specific implementation process includes the following steps: S11. Color images are acquired by a color camera and a depth camera. In this embodiment, an Intel RealSense D435 color depth camera is used to acquire a third-person RGB image of the desktop work area with a resolution of 640×480. S12, Joint angle vectors are fed back in real time by the joint encoder. In this embodiment, the encoder records the angle state vectors of the seven joints of the robotic arm at a frequency of 5Hz.

[0038] S13. Torque feedback is obtained from a six-dimensional torque sensor. In this embodiment, the torque sensor returns joint force data in real time.

[0039] S14, Combining Natural Language Task Description In this specific example, the set of sensory states is formed by "picking up the water glass and then pouring the water into the bowl". .

[0040] S2, the robotic arm control host adopts a ROS2-based distributed communication architecture to publish the sensed state set through ROS2 topics; specifically: The system uses C++ running on Ubuntu 22.04 and subscribes to camera and encoder topics via the ROS2 framework, including image streams, joint angle vectors, and force feedback signals. The rclcpp::Publisher mechanism is used to publish data streams to the / vla_observation topic at a message frequency of 5Hz. Image messages are compressed and encoded into sensor_msgs / Image type, and status messages are encapsulated into Float64MultiArray type to ensure real-time transmission and data synchronization.

[0041] S3. The robotic arm control slave responsible for VLA model inference receives the published ROS2 topic and outputs task-related action commands to the robotic arm control host through the VLA model; the specific implementation includes the following steps: The inputs to the S31 and VLA models are Visual-language encoder via VLA model Then, the joint embedding vector is obtained.

[0042] S32, VLA model action decoder embed vector Mapped to continuous action instructions As shown in the following formula

[0043] in, For the translation increment of the end effector; This represents the increment of the end attitude angle; This represents the opening and closing state of the grippers, where 1 indicates closed and 0 indicates open.

[0044] S4. After receiving the motion command, the control node maps it to a control quantity in the joint space of the robotic arm; the specific implementation includes the following steps: S41. Based on the current joint angle state, the control node obtains the current end-effector pose matrix through the forward kinematics function FK. As shown in the following formula:

[0045] The forward kinematics are established based on the Denavit-Hartenberg (DH) parameters of the robotic arm and are obtained through numerical calculation, as follows: In this embodiment, the forward kinematics model of the robotic arm adopts a DH parameter-based modeling approach. For the i-th joint, its corresponding homogeneous transformation matrix is... Defined as:

[0046] in, Let be the rotation angle of the i-th joint. For the previous link z Offset in the axial direction, The distance between two adjacent joint axes. The torsion angle between two adjacent joint axes.

[0047] The end-effector pose matrix is ​​obtained by multiplying the homogeneous transformation matrices of each link, i.e.:

[0048] Where N is the number of joints in the robotic arm.

[0049] S42, Control node extracts translation increment and in the current end pose matrix Construct the target end pose matrix based on Translation part The specific construction method is as follows: Current end pose matrix As shown in the following formula:

[0050] in, It is a 4×4 homogeneous transformation matrix. The current rotation matrix is ​​3×3; Let the position vector of the current end effector in the base coordinate system be denoted as ; then,

[0051] That is, to obtain the target end pose matrix The translation part is as follows: .

[0052] S43, Control node extracts attitude angle increment and in the current end pose matrix Construct the target end pose matrix based on Rotating part The specific construction method is as follows: First, from the current rotation matrix Solve the current Euler angles using the inverse solution. The target Euler angles are obtained by superimposing the attitude angle increments, as shown in the following formula:

[0053]

[0054]

[0055] Then, the target Euler angles are converted into the target rotation matrix using the Euler angle transformation rotation matrix function. This function converts Euler angles into a 3×3 rotation matrix in ZYX rotation order, as follows: Define three Euler angles, where the roll angle is... φ The pitch angle is θ yaw angle is ψ Then the target rotation matrix for:

[0056] in,

[0057]

[0058]

[0059] Finally, the target end pose matrix is ​​obtained. The rotating part is as follows: .

[0060] S44, Control node based on target end pose With the current joint angle vector As initial values, the numerical iterative inverse kinematics algorithm (IK) is called to solve for the target joint angle vector. As shown in the following formula:

[0061] S45. A numerical iterative method based on minimizing the end-effector pose error is adopted to improve the stability and continuity of the inverse kinematics algorithm solution, as shown in the following equation:

[0062]

[0063] in, This is the current joint angle increment vector. Let Jacobi be the generalized inverse of the Jacobian matrix. and These represent the current and target end poses, respectively. Δt To control the cycle.

[0064] S5. The control layer sends the control quantities of the robotic arm's joint space to the servo driver to realize the actual movement of the robotic arm, and updates the control gain through real-time torque feedback. The specific implementation includes the following steps: S51, Target joint angle The data is sent to the servo driver to realize the actual movement of the robotic arm; S52, Real-time monitoring of position deviation With torque feedback As shown in the following formula:

[0065]

[0066] in, It is the 6-dimensional vector of the target's end pose. It is the 6-dimensional vector of the current end pose. For the first t Real-time torque feedback from the robotic arm. The torque signal is acquired by the torque sensor; S53. Update the control gain based on the feedback data, as follows:

[0067]

[0068] in, For the first t The proportional controller gain at time +1 For the first t The proportional controller gain at time 10:00 This is the proportional gain adjustment coefficient. For the first t Differential controller gain at time +1 For the first t The differential controller gain at time t, This is the differential gain adjustment coefficient.

[0069] The above approach precisely aligns the unified decision-making of the VLA model with the robot's unique hardware teaching logic. Instead of forcibly using a single, potentially inaccurate, inverse solution to handle all actions, it creatively decomposes the actions and executes each action using the most reliable method closest to the source of data acquisition. This hybrid control strategy not only solves the problem of inaccurate rotational movements but also ensures the precision of translational movements. Ultimately, this enables the VLA model to truly empower high-load robots to complete complex physical interaction tasks, effectively improving the autonomy and intelligence of elderly care robots and enabling them to handle complex and varied nursing tasks. It possesses high application value and promising prospects for widespread adoption.

[0070] To better illustrate the technical effects of this embodiment, experiments were conducted in a real-world environment. Experiments verified that this method can achieve high-precision control in typical nursing tasks such as "pouring water," "feeding," and "assisting." The end-effector position error is less than 1.2 mm, the posture error is less than 0.5°, and it remains stable and vibration-free even under a maximum load of 30 kg. This embodiment deeply integrates a large-scale pre-trained visual language model with traditional kinematic control, achieving autonomous conversion from natural language to mechanical movements, significantly improving the intelligence and human-machine collaboration level of the elderly care robot.

[0071] Example 2 It should be further explained that, based on the same inventive concept, this embodiment provides a system for controlling the motion of a large-load robotic arm. When the system is running, it executes the method described in Embodiment 1, including the following modules: The status and task acquisition module is used by the robotic arm control host to receive environmental information, the current state of the robot, and natural language task descriptions collected in real time by multimodal sensors, forming a perception state set; The topic subscription module is used by the robotic arm control host to publish the sensed state set through ROS2 topics using a ROS2-based distributed communication architecture. The action instruction module is used by the robotic arm control slave responsible for VLA model inference to receive published ROS2 topics and output task-related action instructions to the robotic arm control host through the VLA model. The control quantity mapping module is used to map the motion commands received by the control node into control quantities in the joint space of the robotic arm. The control gain adjustment module is used by the control layer to send the control quantity of the robotic arm joint space to the servo driver to realize the actual movement of the robotic arm, and to update the control gain through real-time torque feedback.

[0072] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for motion control of a high-load robotic arm, characterized in that, Includes the following steps: S1. The robotic arm control host receives environmental information, the current state of the robot, and a natural language task description collected in real time by multimodal sensors, forming a perception state set; S2, the robotic arm control host adopts a ROS2-based distributed communication architecture to publish the perception state set through ROS2 topics; S3, the robotic arm control slave responsible for VLA model inference receives the published ROS2 topic and outputs task-related action instructions to the robotic arm control host through the VLA model; S4. After receiving the action command, the control node maps it into the control quantity of the robotic arm joint space. S5. The control layer sends the control quantities of the robotic arm joint space to the servo driver to realize the actual movement of the robotic arm, and updates the control gain through real-time torque feedback.

2. The method for motion control of a large-load robotic arm according to claim 1, characterized in that, The multimodal sensor mentioned in step S1 includes a color camera, a depth camera, a six-dimensional torque sensor, and a joint encoder.

3. The method for motion control of a large-load robotic arm according to claim 2, characterized in that, Step S1 includes the following steps: S11. Color images are acquired by a color camera and a depth camera. ; S12, Joint angle vectors are fed back in real time by the joint encoder. ; S13. Torque feedback is obtained from a six-dimensional torque sensor. ; S14, Combining Natural Language Task Description Forming a set of perceived states .

4. The method for motion control of a large-load robotic arm according to claim 1, characterized in that, Step S2 specifically involves: The system uses C++ running on Ubuntu 22.04 and subscribes to camera and encoder topics via the ROS2 framework, including image streams, joint angle vectors, and force feedback signals. The rclcpp::Publisher mechanism is used to publish data streams to the / vla_observation topic at a message frequency of 5Hz. Image messages are compressed and encoded into sensor_msgs / Image type, and status messages are encapsulated into Float64MultiArray type to ensure real-time transmission and data synchronization.

5. The method for motion control of a large-load robotic arm according to claim 3, characterized in that, Step S3 includes the following steps: The inputs to the S31 and VLA models are Visual-language encoder via VLA model Then, the joint embedding vector is obtained. S32, VLA model action decoder embed vector Mapped to continuous action instructions As shown in the following formula in, For the translation increment of the end effector; This represents the terminal attitude angle increment. This represents the opening and closing state of the grippers, where 1 indicates closed and 0 indicates open.

6. The method for motion control of a large-load robotic arm according to claim 5, characterized in that, Step S4 includes the following steps: S41. Based on the current joint angle state, the control node obtains the current end-effector pose matrix through the positive kinematic function FK. As shown in the following formula: S42, Control node extracts translation increment and in the current end pose matrix Construct the target end pose matrix based on Translation part ; S43, Control node extracts attitude angle increment and in the current end pose matrix Construct the target end pose matrix based on Rotating part ; S44, The control node is based on the target end-effector pose. With the current joint angle vector As initial values, the numerical iterative inverse kinematics algorithm (IK) is called to solve for the target joint angle vector. As shown in the following formula: S45. A numerical iterative method based on minimizing the end-effector pose error is adopted to improve the stability and continuity of the inverse kinematics algorithm solution, as shown in the following equation: in, This is the current joint angle increment vector. Let Jacobi be the generalized inverse of the Jacobian matrix. and These represent the current and target end poses, respectively. Δt To control the cycle.

7. The method for motion control of a large-load robotic arm according to claim 1, characterized in that, Step S42 describes constructing the target end pose matrix. Translation part Specifically: Current end pose matrix As shown in the following formula: in, It is a 4×4 homogeneous transformation matrix. The current rotation matrix is ​​3×3; Let the position vector of the current end effector in the base coordinate system be denoted as ; then, That is, to obtain the target end pose matrix The translation part is as follows: 。 8. The method for motion control of a large-load robotic arm according to claim 1, characterized in that, Step S43 describes constructing the target end pose matrix. Rotating part Specifically: First, from the current rotation matrix Solve the current Euler angles using the inverse solution. The target Euler angles are obtained by superimposing the attitude angle increments, as shown in the following formula: Then, the target Euler angles are converted into the target rotation matrix using the Euler angle transformation rotation matrix function. Obtain the target end pose matrix The rotating part is as follows: 。 9. The method for motion control of a large-load robotic arm according to claim 1, characterized in that, Step S5 includes the following steps: S51, Target joint angle The data is sent to the servo driver to realize the actual movement of the robotic arm; S52, Real-time monitoring of position deviation With torque feedback As shown in the following formula: in, It is the 6-dimensional vector of the target's end pose. It is the 6-dimensional vector of the current end pose. For the first t Real-time torque feedback from the robotic arm. The torque signal is acquired by the torque sensor; S53. Update the control gain based on the feedback data, as follows: in, For the first t The proportional controller gain at time +1 For the first t The proportional controller gain at time 10:00 This is the proportional gain adjustment coefficient. For the first t Differential controller gain at time +1 For the first t The differential controller gain at time t, This is the differential gain adjustment coefficient.

10. A system for motion control of a high-load robotic arm, characterized in that, Includes the following modules: The status and task acquisition module is used by the robotic arm control host to receive environmental information, the current state of the robot, and natural language task descriptions collected in real time by multimodal sensors, forming a perception state set; The topic subscription module is used by the robotic arm control host to publish the sensed state set through ROS2 topics using a ROS2-based distributed communication architecture. The action instruction module is used by the robotic arm control slave responsible for VLA model inference to receive published ROS2 topics and output task-related action instructions to the robotic arm control host through the VLA model. The control quantity mapping module is used to map the motion commands received by the control node into control quantities in the joint space of the robotic arm. The control gain adjustment module is used by the control layer to send the control quantity of the robotic arm joint space to the servo driver to realize the actual movement of the robotic arm, and to update the control gain through real-time torque feedback.

Images