Robot joint control method and system based on motion capture equipment

Abstract

The invention discloses a robot joint control method and system based on motion capture equipment, and belongs to the technical field of robot control. An existing robot joint control scheme lacks an effective man-machine cooperation and natural interaction mechanism, so that the flexibility of a robot is poor, and the intention of an operator cannot be naturally expressed. According to the robot joint control method based on the motion capture equipment, by constructing a motion capture module, a data processing module, a coordinate system transformation module and a solution optimization module, rotating posture data, collected by the motion capture equipment, of human body joints are converted into target joint control information needed by execution of a robot; the robot joint control based on the motion capture equipment is realized, so that the human motion can be efficiently and accurately mapped to the robot, the robot is good in flexibility, sudden motion or complex cooperative operation can be effectively processed, the intention of an operator can be naturally expressed, and man-machine cooperation and natural interaction are realized.

Description

Technical Field

[0001] The present invention relates to a robot joint control method and system based on motion capture equipment, belonging to the technical field of robot control. Background Art

[0002] A Chinese paper (Kuang Yiling, Wu Di, Hou Guowei, et al. A multi-objective optimization method for analytical inverse kinematics of redundant manipulators [J]. Mechanical Science and Technology, 2024, 43 (6): 934-942) discloses a multi-objective optimization method. This method combines the parameterized joint method to improve the multi-objective evolutionary algorithm MOEA / D to form the IM-IK algorithm (ImprovedMOEA / D-inverse kinematics) to solve the inverse kinematics problem of redundant manipulators. This method adds an adaptive interval search strategy to the multi-objective evolutionary algorithm MOEA / D framework to enhance the algorithm's local search capability and improve the solution speed. It can ensure the continuity of the manipulator joint motion and effectively avoid the joint limit while meeting the posture accuracy requirements of the manipulator end effector.

[0003] Although the above solution can realize the intelligent motion control of the multi-degree-of-freedom boom of engineering machinery, it lacks effective human-machine collaboration and natural interaction mechanisms, and does not involve how to efficiently and accurately map human movements to the robot. As a result, the robot has poor flexibility, difficulty in handling sudden movements or complex collaborative operations, inability to naturally express the operator's intentions, and difficulty in achieving high-precision synchronous control of the robot's arms and manipulators.

[0004] Furthermore, this solution and existing robot data collection solutions lack the efficient capture and accurate restoration of natural human movements, resulting in insufficient training data for human-machine collaboration and limiting the development of intelligent systems.

[0005] The information disclosed in this Background Art is only for understanding the background of the present inventive concept and therefore it may include information that does not constitute prior art. Summary of the Invention

[0006] In response to the above problem or one of the above problems, an object of the present invention is to provide a robot joint control method and system based on a motion capture device. By constructing a motion capture module, a data processing module, a coordinate system transformation module, and a solution optimization module, the rotational posture data of the human joints collected by the motion capture device is converted into the target joint control information required for the robot to execute, thereby realizing robot joint control based on the motion capture device, so that human movements can be efficiently and accurately mapped to the robot, making the robot flexible and able to effectively handle sudden movements or complex collaborative operations, thereby naturally expressing the operator's intentions, realizing human-machine collaboration and natural interaction, and facilitating high-precision synchronous control of the robot's arms and manipulators; at the same time, efficient and accurate data collection can be achieved, so that human natural movements can be efficiently captured and accurately restored.

[0007] In response to the above problem or one of the above problems, the second purpose of the present invention is to provide a robot joint control method and system based on motion capture equipment, which can achieve high-fidelity, low-latency mapping of human upper limb and hand movements, effectively improve the control flexibility and accuracy of the robot in complex operation tasks, and at the same time achieve efficient capture and accurate restoration of human natural movements, providing real training data for robots, and has significant practical value and engineering promotion potential.

[0008] To achieve one of the above purposes, the first technical solution of the present invention is: A robot joint control method based on a motion capture device comprises the following steps: Step 1: Using the motion capture module created in advance, the rotational posture data of the human joints collected by the motion capture device is received; Step 2: Use the previously created data processing module to convert the rotational posture data into a unified rotation matrix, and combine it with the human body structure information to calculate the global posture of each joint to obtain posture information; Step 3: Using the previously created coordinate system transformation module, the pose information is converted from the motion capture device coordinate system to the robot operating system coordinate system and the robot body coordinate system in sequence to obtain the standard pose information of the robot; In step 4, the previously created solution optimization module is used to perform posture inverse analysis on the human joints based on the standard posture information, and the target joint control information required for the robot execution is obtained to realize the robot joint control based on the motion capture device.

[0009] The present invention constructs a motion capture module, a data processing module, a coordinate system transformation module, and a solution optimization module to convert the rotational posture data of human joints collected by the motion capture equipment into the target joint control information required for the robot to execute, thereby realizing robot joint control based on the motion capture equipment. This can efficiently and accurately map human movements to the robot, making the robot flexible and able to effectively handle sudden movements or complex collaborative operations; thus, it can naturally express the operator's intentions, realize human-machine collaboration and natural interaction; and facilitate high-precision synchronous control of the robot's arms and manipulators.

[0010] The present invention can not only perform efficient data collection on real robots, but also accurately control robots in simulation environments to achieve efficient data collection, thereby efficiently capturing and accurately restoring human natural movements.

[0011] Furthermore, the present invention utilizes motion capture equipment to collect rotational posture data of human joints in real time and converts it into structured target joint control information, thereby achieving efficient and accurate human motion data collection and mapping. The present invention can provide high-quality training data for learning, verifying, and optimizing robot control strategies, and has advantages such as natural human-computer interaction and strong system scalability. The present invention can not only be used to control real robots for efficient data collection, achieving efficient capture and accurate restoration of natural human motion; it can also directly control robots in simulation environments for synchronous data collection, providing a solid data foundation for intelligent behavior modeling, human-computer collaboration, and virtual-reality fusion research under complex tasks.

[0012] As preferred technical measures: Step 1: The method for receiving the rotational posture data of the human joints collected by the motion capture device through the motion capture module created in advance is as follows: Build a data transmission channel for connecting the motion capture module and the motion capture device through network or Bluetooth communication; Using motion capture equipment to collect the operator's upper limb and hand posture data to obtain motion capture data; Receiving motion capture data including local rotation information of each joint of the operator through a data transmission channel; Based on the type of human joints, build a mapping table and data sharing array; Based on the mapping table, the local rotation information of each joint is stored in the data sharing array, and the joint index is set according to the storage location of the local rotation information to obtain the rotation posture data.

[0013] As preferred technical measures: In step 2, the previously created data processing module is used to convert the rotational posture data into a unified rotation matrix. Combined with the human body structure information, the global posture of each joint is calculated. The method for obtaining the posture information is as follows: Based on the data sharing array and joint index, the local rotation information of each joint is extracted from the rotation posture data; Convert the local rotation information into quaternion form to obtain the pose quaternion; According to the pose quaternion, the rotation matrix is ​​calculated; According to the topological structure of the human upper limb skeleton, key control joints are selected, including but not limited to the hip, multi-segment spine, shoulder, upper arm, forearm and hand; Based on the rotation matrix and key control joints, forward kinematics is used to calculate the global pose of each joint of the human body, which includes at least the pose of the end of the arm and the pose of the end of the finger; Transform the arm end posture into the local coordinate system with the hip as the origin to obtain the arm posture information; Transform the finger end posture into the local coordinate system with the wrist as the origin to obtain the finger posture information; The arm posture information and the finger posture information are aggregated to obtain the posture information.

[0014] As preferred technical measures: The method of converting local rotation information into quaternion form and obtaining the pose quaternion is as follows: Step 11: Based on the conversion relationship between degrees and radians, the local rotation information is processed to calculate the posture radian data; Step 12: Process the attitude radian data according to the sine and cosine calculation formula to calculate the cosine and sine values ​​of each axis; Step 13: Based on the cosine and sine values ​​of each axis and the rotation order of each axis, calculate the pose quaternion.

[0015] As preferred technical measures: Based on the rotation matrix and key control joints, the method of using forward kinematics to calculate the global pose of each joint of the human body is as follows: Step 21, obtaining a rotation matrix, which at least includes a global rotation matrix of the hip joint and a local rotation matrix of the spinal joint; Step 22, multiplying the global rotation matrix by the local rotation matrix to obtain the global rotation matrix of the spinal joint; Step 23: Based on the global rotation matrix of the spinal joints and combined with the topological relationship of the key control joints, the global rotation matrix of the arm end is calculated to complete the rotation construction of the entire upper limb chain; Step 24: Based on the global rotation matrix, the global pose of each joint is calculated based on forward kinematics and the spatial offset vectors between joints.

[0016] As preferred technical measures: The method to transform the arm end posture into the local coordinate system with the hip as the origin to obtain the arm posture information is as follows: According to the global position difference between the arm and the hip, the offset from the arm joint to the hip joint in the global coordinate system is obtained; Based on the offset and the inverse transformation of the hip rotation matrix, the position information of the arm in the hip coordinate system is obtained; Multiply the arm rotation matrix by the inverse matrix of the hip rotation matrix to obtain the arm pose matrix in the hip coordinate system; The arm position information is combined with the arm posture matrix to obtain the arm posture information and complete the transformation of the arm end posture.

[0017] As preferred technical measures: Step 3: Use the previously created coordinate system transformation module to transform the pose information from the motion capture device coordinate system to the robot operating system coordinate system and the robot body coordinate system in sequence. The method for obtaining the standard pose information of the robot is as follows: According to the difference between the coordinate system of the motion capture device and the coordinate system of the robot operating system, a transformation matrix between the coordinate system of the robot operating system and the coordinate system of the motion capture device is constructed; Based on the transformation matrix, the pose information output by the motion capture device is converted into the arm end pose in the robot operating system coordinate system; The robot body coordinate system is a local coordinate system, which includes the left arm coordinate system and the right arm coordinate system; Constructing a first rotation transformation matrix and a second rotation transformation matrix according to the difference between the robot operating system coordinate system and the left arm coordinate system; According to the difference between the robot operating system coordinate system and the right arm coordinate system, the rotation transformation matrix 1 and the rotation transformation matrix 2 are constructed; Based on the first rotation transformation matrix, the second rotation transformation matrix, the rotation transformation matrix one and the rotation transformation matrix two and the end arm posture in the robot operating system coordinate system, the end arm posture in the robot body coordinate system is calculated. Taking the wrist joint as the reference origin, the finger end pose coordinate system is transformed and appropriately scaled based on the finger length to obtain the finger end pose in the robot body coordinate system. The arm end pose and finger end pose in the robot body coordinates are coupled to obtain the robot's pose standard information.

[0018] Furthermore, the motion capture device coordinate system is a left-handed coordinate system, with its X-axis pointing to the right, Y-axis pointing upward, and Z-axis pointing forward; the robot operating system coordinate system is a right-handed coordinate system, with its X-axis pointing forward, Y-axis pointing to the right, and Z-axis pointing upward.

[0019] As preferred technical measures: Step 4: Use the previously created solution optimization module to perform inverse posture analysis on the human joints based on the standard posture information. The method to obtain the target joint control information required for the robot execution is as follows: Obtain standard posture information, which includes the arm end posture and finger end posture in the robot body coordinates; Convert the arm end pose into Euclidean group data; The objective function is constructed with the current robot state as the initial value and the target posture as the optimization target; Based on the Euclidean group data, the objective function is solved to obtain the joint angle solution that meets the constraints; Obtain the degree of freedom information of the robot's fingers and the current posture of the fingers; According to the degree of freedom information, the finger joint limit conditions and inter-finger coordination constraint information are determined; Based on the finger end position, current finger posture, joint limit conditions and inter-finger coordination constraint information, the five-finger angle control information is obtained; The joint angle solution and the five-finger angle control information are coupled to obtain the target joint control information required for robot execution.

[0020] As preferred technical measures: It also includes a control and delivery module; The control issuing module is used to generate control instructions and control the movement of robot joints. The method is as follows: Generate the control instructions required for the robot to execute according to the target joint control information; Send control commands to the robot at a frequency of 500Hz through the communication protocol to control the robot in real time; When the robot communication is interrupted or abnormal, the control instructions of the previous frame are used to maintain the continuity of the robot's movement; When no new control instructions are received within the set time, the robot is controlled to enter a safe state.

[0021] By utilizing the control sending module, the present invention can interact not only with real robots, but also with simulated robots, thereby significantly improving data collection efficiency and realizing multi-scene data collection, increasing data diversity, and enriching robot training data.

[0022] To achieve one of the above purposes, the second technical solution of the present invention is: A robot joint control system based on motion capture equipment, including a motion capture module, a data processing module and a solution optimization module; The motion capture module is used to receive the rotation posture data of the human body joints collected by the motion capture device and store the rotation posture data in the shared memory; The data processing module is used to retrieve the rotation posture data from the shared memory, convert the rotation posture data into a unified rotation matrix, and calculate the global posture of each joint in combination with the human body structure information to obtain the posture information; the posture information is then converted from the motion capture device coordinate system to the robot operating system coordinate system and the robot body coordinate system in sequence to obtain the standard posture information of the robot; The solution optimization module is used to perform posture inverse analysis on human joints based on standard posture information to obtain the target joint control information required for robot execution.

[0023] The present invention sets a motion capture module, a data processing module and a solution optimization module, and uses a low-latency communication mechanism based on shared memory to achieve high-fidelity, low-latency mapping of human upper limb and hand movements, thereby facilitating real-time control of the robot; at the same time, combined with the rotation posture data conversion mechanism and the coordinate system conversion mechanism, the human-machine posture is kept consistent, thereby effectively improving the control accuracy; and furthermore, natural human body movement reproduction and remote control can be achieved, making the robot joint control intuitive; at the same time, efficient and accurate data collection can be achieved, providing the robot with real training data, thereby significantly improving the efficiency of robot data collection and actual deployment, and having high engineering value.

[0024] To achieve one of the above purposes, the third technical solution of the present invention is: A server comprising: one or more processors; a storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned robot joint control method based on the motion capture device.

[0025] Compared with the existing technical solutions, the present invention has the following beneficial effects: The present invention constructs a motion capture module, a data processing module, a coordinate system transformation module, and a solution optimization module to convert the rotational posture data of human joints collected by the motion capture equipment into the target joint control information required for the robot to execute, thereby realizing robot joint control based on the motion capture equipment. This can efficiently and accurately map human movements to the robot, making the robot flexible and able to effectively handle sudden movements or complex collaborative operations. Therefore, it can naturally express the operator's intentions, realize human-machine collaboration and natural interaction, and facilitate high-precision synchronous control of the robot's arms and manipulators.

[0026] Furthermore, the present invention can achieve high-fidelity, low-latency mapping of human upper limb and hand movements, effectively improving the control flexibility and precision of robots in complex operating tasks. At the same time, it can achieve efficient and accurate data collection, thereby efficiently capturing and accurately restoring natural human movements, providing real training data for robots, and has significant practical value and engineering promotion potential.

[0027] Furthermore, the present invention utilizes motion capture equipment to collect real-time rotational posture data of human joints and converts it into structured target joint control information, thereby achieving efficient and accurate human motion data collection and mapping. This method can provide high-quality training data for learning, verifying, and optimizing robot control strategies, and has advantages such as natural human-computer interaction and strong system scalability. The present invention can not only be used to control real robots for efficient data collection, but can also directly control robots in simulated environments for synchronous data collection, providing a solid data foundation for intelligent behavior modeling, human-computer collaboration, and virtual-reality fusion research under complex tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 A schematic flow chart of a robot joint control method based on a motion capture device according to the present invention; Figure 2 This is a structural block diagram of the robot joint control system based on the motion capture device of the present invention. DETAILED DESCRIPTION

[0029] In order to enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only embodiments of a part of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without making creative work should fall within the scope of protection of this application. The present invention covers any substitution, modification, equivalent method and scheme made on the essence and scope of the present invention as defined by the claims.

[0030] like Figure 1 As shown, a specific embodiment of the robot joint control method based on the motion capture device of the present invention is as follows: A robot joint control method based on a motion capture device comprises the following steps: Step 1: Using the motion capture module created in advance, the rotational posture data of the human joints collected by the motion capture device is received; Step 2: Use the previously created data processing module to convert the rotational posture data into a unified rotation matrix, and combine it with the human body structure information to calculate the global posture of each joint to obtain posture information; Step 3: Using the previously created coordinate system transformation module, the pose information is converted from the motion capture device coordinate system to the robot operating system coordinate system and the robot body coordinate system in sequence to obtain the standard pose information of the robot; In step 4, the previously created solution optimization module is used to perform posture inverse analysis on the human joints based on the standard posture information, and the target joint control information required for the robot execution is obtained to realize the robot joint control based on the motion capture device.

[0031] like Figure 2 As shown, a specific embodiment of the robot joint control system based on the motion capture device of the present invention is as follows: A robot joint control system based on a motion capture device comprises a motion capture device, a motion capture module, a data processing module, a solution optimization module and a control distribution module.

[0032] The motion capture device is used to collect data on the operator's upper limb and hand postures. The motion capture module first establishes a connection with the device's data transmitter via TCP / UDP or Bluetooth. It then receives and decodes the data packets (motion capture data) to obtain the real data. Finally, the decoded real data is written to shared memory (a shared data array) for use by the data processing module.

[0033] The data processing module includes a data format conversion module, a posture calculation module and a coordinate system transformation module. The data format conversion module obtains motion capture data from the shared memory and processes the data generated by different motion capture devices into a unified data format; the posture calculation module reconstructs the end posture information of the human upper limbs and hands based on the local posture data of each joint provided by the motion capture system, providing input for subsequent coordinate transformation and inverse kinematics solution; the coordinate system transformation module maps the coordinate system defined by the motion capture device of the human body posture to the coordinate system of the robot body.

[0034] The solution optimization module inversely solves the joint angles based on the target end-point pose; the control distribution module transmits these calculated joint angles to the robot for execution via the DDS communication protocol. The DDS communication design offers strong versatility, allowing the system to interact not only with real robots but also with simulated robots. This significantly improves data collection efficiency and enables multi-scenario data collection, increasing data diversity and enriching the robot's training data.

[0035] In this embodiment, the data format conversion module can uniformly process the different data formats (such as quaternions, rotation matrices, and Euler angles) generated by different motion capture devices into the form of rotation matrices. The processing process is as follows: First, convert the data into quaternion form, and then calculate the rotation matrix based on the quaternion. The process of calculating the quaternion is as follows: Step 1: Convert the received degree information (angle) into radian information (rad) using the following formula:

[0036] Step 2: Use the calculated radian information (rad) to calculate the half-angle cosine value (c) and half-angle sine value (s) of each axis using the following formula:

[0037]

[0038] Step 3: Calculate the quaternion components according to the rotation order (XYZ, XZY, ZXY, ZYX, YZX, YXZ) When calculating quaternion components, calculations are performed according to different rotation orders. Since this embodiment uses the XYZ rotation order, only the calculation formula for the XYZ rotation order is listed below:

[0039]

[0040]

[0041]

[0042] Among them, X represents the X-axis of the coordinate system, Y represents the Y-axis of the coordinate system, and Z represents the Z-axis of the coordinate system; represents the x component of the quaternion, the y component of the quaternion, represents the z component of the quaternion, Represents the numerical value of the scalar part; Represents the cosine value of the half angle around the X axis, Represents the cosine value of the half angle around the Y axis, Represents the cosine value of the half angle around the Z axis; Represents the sine of the half angle around the X axis, Represents the sine of the half angle around the Y axis, Indicates the sine of the half angle around the Z axis.

[0043] Step 4: After the calculation is completed, return the adjusted quaternion After obtaining the quaternion, the corresponding rotation matrix can be obtained by the following formula :

[0044] In this embodiment, the posture calculation module uses forward kinematics to calculate the global posture of each joint of the human body, and then transforms the posture of the end of the arm into a local coordinate system with the hip as the origin, and transforms the posture of the end of the finger into a local coordinate system with the wrist as the origin.

[0045] The calculation process of the arm end pose is as follows: Step 1: Set the joint index and offset to be used. The joint index refers to the position of the required joint in the data sharing array, and the offset refers to the distance between the joints.

[0046] Step 2: Based on the topological structure of the human upper limb skeleton, key control joints are selected, including but not limited to the hip, spine (multiple segments), shoulder, upper arm, forearm, and hand. Each joint has a unique label or index in the motion capture data. The system aligns this with the original data using a mapping table and uniformly converts it into a rotation matrix or quaternion format to obtain the local rotation matrix for each joint. This approach allows the system to be compatible with the output structures of different motion capture devices and ensures that the rotation poses of all participating joints are consistent and comparable.

[0047] Step 3: The motion capture device typically provides local rotation information for each joint, i.e., the rotation of the joint relative to its parent node. To obtain the global rotation matrix of each joint in the world coordinate system, the system performs matrix concatenation calculations based on the hierarchical structure of the human skeleton to obtain the global rotation matrix. The specific method is as follows: The global rotation matrix of the spine joints is obtained by multiplying the global rotation matrix of the hips by the local rotation of the spine. This process is recursively expanded from the root node (such as the hip joint) to the end of the hand, completing the rotation construction of the entire upper limb chain.

[0048] Taking the calculation of the spine joint as an example, we need to calculate the global rotation matrix relative to the hip joint. The calculation formula is as follows:

[0049] in, is the global rotation matrix of the spine joint, is the global rotation matrix of the hip joint, is the local rotation matrix of the spine joint.

[0050] Through similar calculations, we can finally get the global rotation matrix of the end of the arm .

[0051] Step 4: Calculate the global pose of each joint based on its offset. After obtaining the global rotation matrix, the system combines known human anatomy parameters (i.e., the spatial offset vectors between joints) to calculate the global position of each joint. This process relies on the principle of forward kinematics: the position of a child joint is determined by the position of the parent joint, its rotation, and its relative offset.

[0052] The system sets the offset value between each pair of adjacent joints based on the human body standard model or measured calibration data, and superimposes the transformation step by step to finally obtain the global posture information (position + rotation) of the upper limb end (such as the left / right wrist).

[0053] In this embodiment, the formula for calculating the position of the spine joint is as follows:

[0054] in, is the global position of the spine joint; is the global position of the hip joint, usually the origin; is the global rotation matrix of the hip joint; The offset of the spine joint relative to the hip joint needs to be measured; similarly, through level-by-level calculations, the global position of the end of the arm can be calculated. .

[0055] Step 5: To adapt to the robot control framework, the system needs to further transform the position of the end arm into a local coordinate system with the hip as the origin, thereby eliminating the influence of the overall human body displacement on the motion mapping. The transformation process includes the relative offset calculation of the position vector and the coordinate system transformation of the rotation matrix. The position transformation method is as follows: The position of the hand in the hip coordinate system is obtained based on the global position difference between the hand and the hip, combined with the inverse transformation of the hip rotation.

[0056] The method of posture transformation is as follows: The hand rotation matrix is ​​multiplied by the inverse matrix of the hip rotation to obtain its pose in the hip coordinate system.

[0057] For finger joints, the system uses a similar process, but the reference origin is the wrist joint. Because human fingers are generally longer than those of robotic dexterous hands, the system introduces a scaling factor during the end-point posture conversion process to improve the consistency and practicality of the motion mapping. The calculation formula is as follows:

[0058]

[0059]

[0060] in, is the offset from the hand joint to the hip joint in the global coordinate system; The hand joint is the position of the arm in the hip coordinate system; It is the rotation of the hand joint in the hip coordinate system.

[0061] The specific value of the scaling factor is the ratio of the length of a human finger to the length of a robot finger.

[0062] Step 6: Return the arm end position and posture, that is, the calculated above and Feedback to subsequent modules.

[0063] In this embodiment, the finger tip pose calculation process is essentially the same as the arm pose calculation process. However, it's important to note that the finger pose calculation must take into account finger length and transform the finger rotation matrix into a local coordinate system with the wrist as the origin. Because there's a certain difference between human finger length and the length of the robot's dexterous hand, the calculation requires measuring both the actual length of the user's finger and the length of the robot's dexterous hand's fingers for scaling purposes.

[0064] In this embodiment, to effectively connect the motion capture data to the robotic system, a coordinate system transformation module is used to convert the raw data from the coordinate system used by the motion capture device to the control coordinate system used by the robot. Due to differences in axial definition and chirality between the two coordinate systems, a multi-stage transformation is required.

[0065] The motion capture device Unity usually uses a left-handed coordinate system, with the X axis pointing to the right, the Y axis pointing up, and the Z axis forward. The robot operating system ROS and most robot platforms use a right-handed coordinate system, with the X axis pointing forward, the Y axis to the right, and the Z axis upward. The robot body Unitree has its own defined local coordinate system, and its axis settings vary depending on the left and right arms.

[0066] In this embodiment, the coordinate system transformation module converts the arm end pose coordinate system as follows: First, convert the coordinate system of the motion capture device Unity to the coordinate system of the robot operating system ROS. The process is as follows: The pose data output by the motion capture system is converted to pose data in the right-handed coordinate system of the Robot Operating System (ROS) using a predefined rotation transformation matrix. Pose data includes position and corresponding posture. For ease of operation, this embodiment converts position and posture separately. The expression for position conversion is as follows:

[0067] in is the position of the end of the arm in the robot operating system ROS coordinates, is the end position of the arm in the Unity coordinate system of the motion capture device, It is the transformation matrix between the robot operating system ROS coordinate system and the motion capture device Unity coordinate system, and its expression is as follows:

[0068] The expression of posture transformation is as follows:

[0069] in is the end-of-arm posture in the robot operating system ROS coordinates, It is the processed arm end posture in the Unity coordinate system of the motion capture device. It is the transformation matrix between the robot operating system ROS coordinate system and the motion capture device Unity coordinate system, and its expression is as follows:

[0070] Then, the robot operating system ROS coordinate system is converted to the robot Robot coordinate system. The process is as follows: Considering that the left and right arms of the robot have different coordinate axis definitions, the system uses independent rotation transformation matrices for the left and right arms to ensure that the transformed end-point posture meets the robot control input specifications. The expression is as follows:

[0071] in, is the end pose of the arm in Unitree coordinates, and They are all transformation matrices.

[0072] For the left arm coordinate system, the transformation matrix is ​​expressed as follows:

[0073]

[0074] For the right arm coordinate system, the transformation matrix is ​​expressed as follows:

[0075]

[0076] In this embodiment, the coordinate system transformation module converts the finger tip position coordinate system as follows: The coordinate system conversion process for fingers is similar to that for arms, but the reference origin is the wrist joint. Because the structure and degrees of freedom of the robot hand differ from those of the human hand, the conversion process requires appropriate scaling based on the actual finger length to maintain consistent movement.

[0077] The system sets transformation matrices for the left and right hands respectively to ensure that the end position of each finger can be accurately mapped to the control space of the robot finger. The calculation formula is as follows:

[0078] in is the end position of the finger in the robot Unitree coordinates, is the transformation matrix, It is the finger tip position in the Unity coordinate system of the motion capture device.

[0079] The expression of the left-hand transformation matrix is ​​as follows:

[0080] The expression of the right-hand transformation matrix is ​​as follows:

[0081] Therefore, when the left and right arms and the left and right fingers are transformed into the body coordinate system, their transformation matrices are different, which can further improve the robot control accuracy.

[0082] In this embodiment, to achieve accurate tracking of human target motions by the robot's joints, the solution optimization module performs inverse kinematics (IK) based on the end-point pose information, and processes the joint angle calculations of the arms and fingers separately.

[0083] The arm part has 7 degrees of freedom, and the system implements inverse kinematics IK solution based on the Pinocchio library for robot dynamics calculation and CasADi, an open source software tool for dynamic optimization.

[0084] Pinocchio, a library for robot dynamics calculations, can build robot motion models and generate kinematic constraints for each joint; CasADi, an open source software tool for dynamic optimization, can define objective functions and optimization variables and perform numerical optimization solutions.

[0085] The calculation process of the arm joint angle is as follows: First, the target pose (position and rotation matrix) of the end of the arm is converted into the special Euclidean group SE(3) format; Then, an optimization problem is constructed, with the current robot state as the initial value and the target posture as the optimization target, to build an objective function; then, the open source software tool CasADi is used to solve the objective function to obtain the joint angle solution that meets the constraints.

[0086] This method can introduce weighted strategies, joint constraints, and redundant solution determination mechanisms among multiple objectives to ensure that the solution has high feasibility and dynamic stability.

[0087] The open source library dex_retargeting is used to solve the finger inverse kinematics IK. The calculation process is as follows: First, the degrees of freedom of the robot's fingers are obtained. In this embodiment, each five-fingered dexterous hand contains 20 degrees of freedom. Then, a scaling factor is set to adapt to the difference in geometric dimensions between the human hand and the robot hand. The value of the scaling factor is determined according to the ratio of the human hand to the robot hand. Then, based on physical constraints such as joint limits and inter-finger coordination, modeling is performed, and then finger-by-finger optimization is performed according to the target finger end posture and current posture. The target end posture and current posture of each finger are used as input, and the optimizer is called to solve and obtain the complete joint angle output of the five fingers.

[0088] In this embodiment, the control delivery module includes the following contents: The control distribution module sends control commands to the robot system at a frequency of 500Hz via the DDS communication protocol. This not only meets real-time control requirements but also utilizes the previous frame data to maintain motion continuity in the event of communication interruptions or anomalies, preventing robot jitter or instability. Furthermore, the module supports a failback mechanism, automatically entering a safe state if no new control commands are received within a set time, ensuring system stability and personnel safety.

[0089] This invention has been deployed and verified on multiple platforms, including using motion capture equipment such as the Moxun HZ1 and Tianqi F-1 to control robots such as the Unitree G1 (with a three-finger dexterous hand) and the Unitree H1-2 (with a five-finger dexterous hand). Therefore, this invention is compatible with multiple brands of motion capture equipment and robotic platforms, demonstrating its versatility.

[0090] A specific embodiment of the present invention is applied: In this embodiment, the HZ1 motion capture device produced by Moxun Company is selected, and the Yushu humanoid robot H1-2 is used as the controlled platform. The robot is equipped with two seven-degree-of-freedom arms and a five-fingered dexterous hand with twenty degrees of freedom.

[0091] The system communicates with the motion capture device through the UDP communication protocol to receive real-time rotational posture data (expressed in quaternions) of the human upper limbs and hands.

[0092] During system operation, the motion capture module receives and parses UDP packets, writing skeletal joint rotation information to shared memory. The data processing module extracts the corresponding skeletal data from shared memory, converts it into a unified rotation matrix format, and calculates the global pose of each joint based on the human body structure information.

[0093] The coordinate transformation module converts this pose information from the motion capture device's Unity coordinate system to the Robot Operating System (ROS) coordinate system and the Yushu robot's body coordinate system. The solution optimization module performs inverse pose analysis on the dual arms and five-fingered dexterous hand, respectively, to determine the target joint angles required for robot execution.

[0094] Finally, the control distribution module sends the calculation results to the robot's underlying control system through the communication protocol DDS at a frequency of 500Hz, enabling it to perform coordinated movements consistent with human movements, achieving real-time and natural mapping of human movements to robot joints, and successfully completing complex operation tasks such as grasping and gesture demonstration.

[0095] This embodiment verifies that the low-latency communication mechanism based on shared memory of the present invention can achieve real-time response; at the same time, combined with forward / inverse kinematics and proportional scaling, the human-machine posture remains consistent, thereby effectively improving the control accuracy; and furthermore, natural human motion reproduction and remote control can be achieved, making the robot joint control intuitive; and it can significantly improve the efficiency of robot data collection and actual deployment, with high engineering value.

[0096] A server embodiment using the method of the present invention: A server comprising: one or more processors; a storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned robot joint control method based on the motion capture device.

[0097] The module in this application is an object that objectively describes the morphological structure with the help of physical or virtual representation. The object is not equal to the physical body and is not limited to physical and virtual. It can be a data processing function, software program, processing mode, usage method, operation method, workflow, application process, electronic hardware, circuit module, processing system, system imitation or simulation object.

[0098] Finally, it should be noted that the above-described embodiments are only specific implementation methods of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the above-described embodiments, those skilled in the art should understand that any person skilled in the art can modify the technical solutions described in the above-described embodiments within the technical scope disclosed by the present invention, or replace some of the technical features therein with equivalents. However, these modifications or replacements will not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and any modifications or equivalent replacements that do not deviate from the spirit and scope of the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A robot joint control method based on a motion capture device, characterized by: The following steps are involved: Step 1: Using the motion capture module created in advance, the rotational posture data of the human joints collected by the motion capture device is received; Step 2: Use the previously created data processing module to convert the rotational posture data into a unified rotation matrix, and combine it with the human body structure information to calculate the global posture of each joint to obtain posture information; Step 3: Using the previously created coordinate system transformation module, the pose information is converted from the motion capture device coordinate system to the robot operating system coordinate system and the robot body coordinate system in sequence to obtain the standard pose information of the robot; In step 4, the previously created solution optimization module is used to perform posture inverse analysis on the human joints based on the standard posture information, and the target joint control information required for the robot execution is obtained to realize the robot joint control based on the motion capture device.

2. The robot joint control method based on a motion capture device according to claim 1, characterized in that: Step 1: The method for receiving the rotational posture data of the human joints collected by the motion capture device through the motion capture module created in advance is as follows: Build a data transmission channel for connecting the motion capture module and the motion capture device through network or Bluetooth communication; Using motion capture equipment to collect the operator's upper limb and hand posture data to obtain motion capture data; Receiving motion capture data including local rotation information of each joint of the operator through a data transmission channel; Based on the type of human joints, build a mapping table and data sharing array; Based on the mapping table, the local rotation information of each joint is stored in the data sharing array, and the joint index is set according to the storage location of the local rotation information to obtain the rotation posture data.

3. The robot joint control method based on a motion capture device according to claim 1, wherein: In step 2, the previously created data processing module is used to convert the rotational posture data into a unified rotation matrix. Combined with the human body structure information, the global posture of each joint is calculated. The method for obtaining the posture information is as follows: Based on the data sharing array and joint index, the local rotation information of each joint is extracted from the rotation posture data; Convert the local rotation information into quaternion form to obtain the pose quaternion; According to the pose quaternion, the rotation matrix is ​​calculated; According to the topological structure of the human upper limb skeleton, key control joints are selected, including but not limited to the hip, multi-segment spine, shoulder, upper arm, forearm and hand; Based on the rotation matrix and key control joints, forward kinematics is used to calculate the global pose of each joint of the human body, which includes at least the pose of the end of the arm and the pose of the end of the finger; Transform the arm end posture into the local coordinate system with the hip as the origin to obtain the arm posture information; Transform the finger end posture into the local coordinate system with the wrist as the origin to obtain the finger posture information; The arm posture information and the finger posture information are aggregated to obtain the posture information.

4. The robot joint control method based on motion capture equipment according to claim 3, characterized in that: The method of converting local rotation information into quaternion form and obtaining the pose quaternion is as follows: Step 11: Based on the conversion relationship between degrees and radians, the local rotation information is processed to calculate the posture radian data; Step 12: Process the attitude radian data according to the sine and cosine calculation formula to calculate the cosine and sine values ​​of each axis; Step 13: Based on the cosine and sine values ​​of each axis and the rotation order of each axis, calculate the pose quaternion.

5. The robot joint control method based on motion capture equipment according to claim 3, characterized in that: Based on the rotation matrix and key control joints, the method of using forward kinematics to calculate the global pose of each joint of the human body is as follows: Step 21, obtaining a rotation matrix, which at least includes a global rotation matrix of the hip joint and a local rotation matrix of the spinal joint; Step 22, multiplying the global rotation matrix by the local rotation matrix to obtain the global rotation matrix of the spinal joint; Step 23: Based on the global rotation matrix of the spinal joints and combined with the topological relationship of the key control joints, the global rotation matrix of the arm end is calculated to complete the rotation construction of the entire upper limb chain; Step 24: Based on the global rotation matrix, the global pose of each joint is calculated based on forward kinematics and the spatial offset vectors between joints.

6. The robot joint control method based on motion capture equipment according to claim 5, characterized in that: The method to transform the arm end posture into the local coordinate system with the hip as the origin to obtain the arm posture information is as follows: According to the global position difference between the arm and the hip, the offset from the arm joint to the hip joint in the global coordinate system is obtained; Based on the offset and the inverse transformation of the hip rotation matrix, the position information of the arm in the hip coordinate system is obtained; Multiply the arm rotation matrix by the inverse matrix of the hip rotation matrix to obtain the arm pose matrix in the hip coordinate system; The arm position information is combined with the arm posture matrix to obtain the arm posture information and complete the transformation of the arm end posture.

7. The robot joint control method based on motion capture equipment according to claim 1, characterized in that: Step 3: Use the previously created coordinate system transformation module to transform the pose information from the motion capture device coordinate system to the robot operating system coordinate system and the robot body coordinate system in sequence. The method for obtaining the standard pose information of the robot is as follows: According to the difference between the coordinate system of the motion capture device and the coordinate system of the robot operating system, a transformation matrix between the coordinate system of the robot operating system and the coordinate system of the motion capture device is constructed; Based on the transformation matrix, the pose information output by the motion capture device is converted into the arm end pose in the robot operating system coordinate system; The robot body coordinate system is a local coordinate system, which includes the left arm coordinate system and the right arm coordinate system; Constructing a first rotation transformation matrix and a second rotation transformation matrix according to the difference between the robot operating system coordinate system and the left arm coordinate system; According to the difference between the robot operating system coordinate system and the right arm coordinate system, the rotation transformation matrix 1 and the rotation transformation matrix 2 are constructed; Based on the first rotation transformation matrix, the second rotation transformation matrix, the rotation transformation matrix one and the rotation transformation matrix two and the end arm posture in the robot operating system coordinate system, the end arm posture in the robot body coordinate system is calculated. Taking the wrist joint as the reference origin, the finger end pose coordinate system is transformed and appropriately scaled based on the finger length to obtain the finger end pose in the robot body coordinate system. The arm end pose and finger end pose in the robot body coordinates are coupled to obtain the robot's pose standard information.

8. The robot joint control method based on motion capture equipment according to claim 1, characterized in that: Step 4: Use the previously created solution optimization module to perform inverse posture analysis on the human joints based on the standard posture information. The method to obtain the target joint control information required for the robot execution is as follows: Obtain standard posture information, which includes the arm end posture and finger end posture in the robot body coordinates; Convert the arm end pose into Euclidean group data; The objective function is constructed with the current robot state as the initial value and the target posture as the optimization target; Based on the Euclidean group data, the objective function is solved to obtain the joint angle solution that meets the constraints; Obtain the degree of freedom information of the robot's fingers and the current posture of the fingers; According to the degree of freedom information, the finger joint limit conditions and inter-finger coordination constraint information are determined; Based on the finger end position, current finger posture, joint limit conditions and inter-finger coordination constraint information, the five-finger angle control information is obtained; The joint angle solution and the five-finger angle control information are coupled to obtain the target joint control information required for robot execution.

9. The robot joint control method based on motion capture equipment according to claim 1, characterized in that: It also includes a control and delivery module; The control issuing module is used to generate control instructions and control the movement of robot joints. The method is as follows: Generate the control instructions required for the robot to execute according to the target joint control information; Send control commands to the robot at a frequency of 500Hz through the communication protocol to control the robot in real time; When the robot communication is interrupted or abnormal, the control instructions of the previous frame are used to maintain the continuity of the robot's movement; When no new control instructions are received within the set time, the robot is controlled to enter a safe state.

10. A robot joint control system based on a motion capture device, characterized by: Including motion capture module, data processing module and solution optimization module; The motion capture module is used to receive the rotation posture data of the human body joints collected by the motion capture device and store the rotation posture data in the shared memory; The data processing module is used to retrieve the rotation posture data from the shared memory, convert the rotation posture data into a unified rotation matrix, and calculate the global posture of each joint in combination with the human body structure information to obtain the posture information; the posture information is then converted from the motion capture device coordinate system to the robot operating system coordinate system and the robot body coordinate system in sequence to obtain the standard posture information of the robot; The solution optimization module is used to perform posture inverse analysis on human joints based on standard posture information to obtain the target joint control information required for robot execution.

Images