Digital twin robot control method based on motion sensor

Abstract

The application discloses a digital twin mechanical arm control method based on a motion sensor, and comprises the following steps: step one, building a digital twin model of a mechanical arm; step two, establishing wireless communication between the motion sensor and a computer; step three, using a Kalman filter to solve the posture, and solving the motion data of the motion sensor into current posture information; after obtaining the posture information, the angle and displacement of each joint of the mechanical arm at the target posture are calculated through a mechanical arm inverse kinematics algorithm; step four, sending the motion parameters to the controller of the mechanical arm through TCP communication to control the motion of the mechanical arm to the specified position, and unity sends the motion of each life cycle to the digital twin model, so that the digital twin mechanical arm follows the motion of the real mechanical arm. The application solves the life and health problems of workers when they control the mechanical arm at a close distance in a harsh working environment.

Description

Technical Field

[0001] This invention relates to the field of robotic arm control technology, specifically to a control method for a digital twin robotic arm based on motion sensors. Background Technology

[0002] In modern industrial production and maintenance, robotic arms undoubtedly play a crucial role. They not only bring high efficiency and precision to industrial production but also greatly reduce human physical labor. However, traditional robotic arm control methods are often based on programming or manual operation, requiring operators to teach and debug the robotic arm. But in some special situations, such as mines, nuclear power plants, and chemical plants, these environments are extremely dangerous for workers. Traditionally, these tasks rely on manual teaching of the robotic arm. However, this approach poses a significant threat to the operator's life and health. Meanwhile, commercially available robotic arm teaching technologies lack flexibility; once the robotic arm's teaching is complete, its movement trajectory is essentially fixed, making it difficult to flexibly adjust to changes in the environment. When the position of the work object changes, programming and teaching must be restarted from scratch, which is cumbersome and inefficient. Moreover, robotic arm teaching requires operators to have certain professional skills and experience, limiting its widespread adoption. To address these problems, this invention uses motion sensors to control the robotic arm, achieving human-machine separation through digital twin technology, effectively solving the aforementioned issues.

[0003] This method uses motion sensors to transmit parameters such as the three-axis acceleration and angular velocity of the human hand's movements to a computer via TCP communication. After receiving the data from the motion sensors, the computer uses attitude calculation and inverse kinematics algorithms in its software to convert the hand's movements into the angles and velocities of each joint of the robotic arm. These motion parameters are then transmitted to the robotic arm control board via TCP communication, which in turn controls the robotic arm to perform the corresponding movements. Simultaneously, the digital twin model in Unity is also sent to each axis of the robotic arm according to the calculated motion parameters, thus enabling the virtual robotic arm model to follow the movement of the real robotic arm model.

[0004] The innovation of this invention lies primarily in the combination of motion sensors and digital twin technology, proposing a motion sensor-based digital twin robotic arm control method. This method significantly reduces the difficulty and barrier to robotic arm control teaching while ensuring control accuracy. Furthermore, the use of digital twin technology to separate human-machine interaction addresses potential health and safety issues that may arise when operators perform on-site teaching and control operations of the robotic arm in harsh working environments. Summary of the Invention

[0005] The purpose of this invention is to address the shortcomings of traditional robotic arm teaching control by proposing a digital twin robotic arm based on motion sensor control. This invention significantly reduces the difficulty and threshold of robotic arm control teaching while ensuring the accuracy of robotic arm control. Furthermore, through digital twin technology, it enables safe control of the robotic arm in harsh working environments.

[0006] This invention provides a control method for a digital twin robotic arm based on motion sensor control, specifically including the following steps:

[0007] Step 1: Build a digital twin model of the robotic arm, set the motion limits and initial positions of each axis of the controlled robotic arm; establish wireless communication between the digital twin robotic arm and the real robotic arm;

[0008] Step 2: Establish wireless communication between the motion sensor and the computer;

[0009] Step 3: Use a Kalman filter to calculate the attitude, converting the motion data from the motion sensor into the current attitude information. After obtaining the attitude information, use the inverse kinematics algorithm of the robotic arm to calculate the angle and displacement of each joint of the robotic arm in the target attitude;

[0010] Step 4: Motion sensors collect parameters such as three-axis acceleration, angular velocity, and yaw angle of the current hand movement. These parameters are then converted into motion vectors and joint angles for the robotic arm using attitude calculation algorithms and inverse kinematics algorithms in the software. The motion parameters are sent to the robotic arm controller via TCP communication to move the robotic arm to the designated position. Simultaneously, Unity sends the motion data for each lifecycle to the digital twin model, enabling the digital twin robotic arm to follow the movement of the real robotic arm.

[0011] The key feature of this invention is the proposal of a digital twin robotic arm control method based on motion sensors. It employs algorithms for attitude calculation and coordinate transformation to accurately capture human hand movements and convert them into robotic arm movements. This allows for control of complex robotic arm movements using the most intuitive human experience, achieving "what you think is what you get," significantly reducing the difficulty and barrier to robotic arm control teaching. Furthermore, the use of digital twin technology allows for the separation of the robotic arm control unit and the controlled object. Operators can control and teach the robotic arm without being physically present, solving the health and safety issues that arise when workers control robotic arms at close range in harsh working environments. This provides effective technical support for the large-scale application and widespread adoption of robotic arm control teaching.

[0012] Overall, the technical method proposed in this invention has the following advantages compared with the prior art:

[0013] 1. This invention employs motion sensors to control a robotic arm, abandoning the traditional manual teaching method and using the most intuitive human motion perception to control the robotic arm's movement. The robotic arm perfectly replicates the movements of a human hand, achieving "what you think is what you get." This significantly lowers the barrier to entry and difficulty of robotic arm control.

[0014] 2. This invention employs digital twin technology to build a digital twin model of the robotic arm in a virtual environment. Operators can observe the movement of the real robotic arm by viewing the virtual model. This eliminates the need for close observation beside the robotic arm, achieving human-machine separation and facilitating task completion in harsh working environments.

[0015] 3. This invention combines motion sensor control and digital twin technology. By building a virtual model on the Unity platform, motion sensors are used to control the robotic arm, achieving monitoring and intelligent control of the robotic arm. This method introduces a simpler and more intuitive approach to robotic arm control, and has broad application prospects. Attached Figure Description

[0016] Figure 1 This is the overall flowchart of the present invention.

[0017] Figure 2 This is a schematic diagram of a robotic arm controlled by a motion sensor. Detailed Implementation

[0018] The present invention will be described in detail below through specific embodiments.

[0019] This example demonstrates a digital twin robotic arm control method based on motion sensors. The method consists of motion sensors, attitude calculation algorithms, inverse kinematics algorithms for robotic arms, and motion control of the real robotic arm and the digital twin model.

[0020] A specific implementation example includes the following steps:

[0021] Step 1: Build a digital twin model of the robotic arm, set the motion limits and initial positions of each axis of the controlled robotic arm; establish wireless communication between the digital twin robotic arm and the real robotic arm.

[0022] Step 1.1, Figure 2 The robotic arm in the image is a real industrial robotic arm model. The 3D model of the robotic arm is created in the 3D modeling software Solidworks and exported in the common .sld format.

[0023] Step 1.2: Import the exported .sld format 3D model into software such as 3DMax that can convert formats, and then export it in a format that Unity can recognize, such as .fbx or .obj.

[0024] Step 1.3: Import the converted file into Unity and establish constraints and parent-child relationships between each part. Ensure that the joints of each axis of the robotic arm rotate normally and that there is no interference between them.

[0025] Step 1.4: After building the digital twin model of the robotic arm, attach the relevant scripts to the Unity scene and attach function calls to the corresponding entity objects. The entity objects include: ① Frame: The base of the robotic arm, used to fix the robotic arm; this object is locked and cannot move in the Unity scene. ② Robotic Arm: The controlled object, receiving motion commands from Unity to move. The function calls include: ① Initialization function: Automatically called after each power-on to initialize the positions of each joint of the robotic arm and communication parameters. ② Robotic Arm Motion Limit Function: Attached to each axis of the robotic arm; if the robotic arm's movement exceeds its limits or interference occurs, all movement will stop and an error message will pop up. ③ Robotic Arm Motion Function: Sends the motion vectors of each axis of the robotic arm to the robotic arm model. This function is located in the update function of the script and is executed once in each Unity lifecycle to ensure that the robotic arm's movement is updated in real time.

[0026] Step 1.5: Establish socket communication between the digital twin robotic arm and the real robotic arm, using TCP / IP as the communication protocol. The real robotic arm acts as the server, and the digital twin model acts as the client.

[0027] Step 2: Establish wireless communication between the motion sensor and the computer.

[0028] Step 2.1: The motion sensor integrates a Wi-Fi module. Configure the IP address and port number of the Wi-Fi module via a computer.

[0029] Step 2.2: Connect the computer to the Wi-Fi module and set the computer's IP address and port number, ensuring that the computer's IP address and the Wi-Fi module's IP address are in the same network segment.

[0030] Step 2.3: Establish socket communication between the motion sensor and the computer using the TCP / IP protocol. The computer acts as the server, and the motion sensor as the client.

[0031] Step 3: Use the Kalman filter algorithm to calculate the attitude, converting the motion data from the motion sensor into the current attitude information. After obtaining the attitude information, use the inverse kinematics algorithm of the robotic arm to calculate the angle and displacement of each joint of the robotic arm in the target attitude.

[0032] Step 3.1: The main functional components in the motion sensor are the IMU sensor and the magnetometer. The IMU sensor can measure the acceleration and angular velocity of the x, y, and z axes, and the magnetometer can obtain magnetic field information to obtain the current yaw angle.

[0033] Step 3.2: Initialize the state estimate and covariance matrix of the Kalman filter. The state vector can be chosen as [roll, pitch, yaw, gyro_bias_x, gyro_bias_y, gyro_bias_z], where roll and pitch represent the pitch and roll angles measured by the accelerometer, yaw represents the yaw angle measured by the magnetometer, and gyro_bias_x, gyro_bias_y, and gyro_bias_z represent the drift bias of the gyroscope. The covariance matrix can be initialized based on prior knowledge or empirical values.

[0034] Step 3.3, define the measurement model to obtain attitude information from the sensor measurement data. Attitude is measured using data from the accelerometer and magnetometer. The measurement model can be represented as:

[0035] roll mea =arctan(accel y / accel z )

[0036]

[0037] yaw mea =arctan(mag) y ·sin(roll)-mag z ·cos(roll) / mag x ·cos(pitch)

[0038] +mag y sin(pitch) sin(roll) + mag z ·sin(pitch)·cos(roll))

[0039] Among them, accel x accel y accel z The acceleration along the x, y, and z axes is obtained from the accelerometer in the IMU. x mag y mag z These are the magnetic field data along the x, y, and z axes obtained through measurements using a magnetometer. mea pitch mea and yaw meaThe attitude angle is obtained by measuring accelerometers and magnetometers.

[0040] Step 3.4: Based on the system dynamic model, predict the attitude state and covariance matrix at the next moment using the angular velocity data from the gyroscope. According to the measurement model, compare the measurement data from the accelerometer and magnetometer with the predicted values ​​to correct the predicted attitude state and covariance matrix. Repeat the prediction and update steps in each time period, continuously updating the attitude information using new sensor data.

[0041] Step 3.5: Based on the geometric parameters of the robotic arm (length of each joint, joint type, coordinate system definition), establish a forward kinematic model to convert joint angles or displacements into the position and orientation of the end effector.

[0042] Step 3.6: Based on the forward kinematics model, establish the inverse kinematics model of the robotic arm. Convert the attitude and position of the end effector into joint angles.

[0043] Step 3.7: Substitute the attitude information obtained after attitude calculation into the inverse kinematics model as the target attitude to solve for the angle and displacement required for each joint of the robotic arm to move under the target attitude.

[0044] Step 4: Motion sensors collect parameters such as three-axis acceleration, angular velocity, and yaw angle of the current hand movement. These parameters are then converted into motion vectors and joint angles for the robotic arm using attitude calculation algorithms and inverse kinematics algorithms in the software. The motion parameters are sent to the robotic arm controller via TCP communication to move the robotic arm to the designated position. Simultaneously, Unity sends the motion data for each lifecycle to the digital twin model, enabling the digital twin robotic arm to follow the movement of the real robotic arm.

[0045] Step 4.1: Activate the motion sensor. Press the gain button on the control handle to select the control gain. Press the trigger button on the control handle to enter motion control mode. Each time the trigger button is pressed, the motion sensor will establish a spatial coordinate system with the current position as the origin, right as the positive X-axis direction, forward as the positive Y-axis direction, and upward as the positive Z-axis direction. Figure 2 As shown, the acquired motion data is all based on this coordinate system. The handheld motion sensor moves a certain distance, and the IMU and magnetometer within the motion sensor acquire the current triaxial acceleration, angular velocity, and yaw angle information. This information, along with the control gain, is sent to the MCU, which then uploads this data to the computer via Socket communication according to the pre-defined communication protocol.

[0046] Step 4.2: After receiving a data packet, the computer parses the motion data according to the communication protocol, and then uses the Kalman filter algorithm to calculate the attitude and obtain the current attitude information of the motion sensor. Based on the control gain set by the control handle, the displacement vector in the attitude information is multiplied by the gain to obtain the target position, while the angle rotation vector in the attitude information remains unchanged.

[0047] Step 4.3: The attitude information multiplied by the gain is used as the target position to obtain the motion angles and displacements of each axis joint of the robot arm through inverse kinematics calculation.

[0048] Step 4.4: After obtaining the joint motion parameters of the robotic arm, the computer uploads the motion information to the control board of the actual robotic arm via Socket communication. The control board controls the motor movement at each axis joint of the robotic arm. Similar to the motion controller, at the start of each movement, the robotic arm establishes a spatial coordinate system with its current position as the origin, the positive X-axis pointing to the right, the positive Y-axis pointing forward, and the positive Z-axis pointing upward. Figure 2 As shown, the movements of the robotic arm are all based on the coordinate system of the starting position. The displacement vector of the motion sensor in its own coordinate system is multiplied by the motion gain to obtain the displacement vector of the robotic arm's end effector in its own coordinate system, thereby replicating the movements of a human hand.

[0049] Step 4.5: While obtaining the joint motion parameters of the robotic arm, the update function in the Unity script will call the motion parameters of the robotic arm and execute the motion function of the virtual robotic arm in each lifecycle, controlling the digital twin model to move to the specified position, so that the digital twin model follows the movement of the real robotic arm.

Claims

1. A method for controlling a digital twin robot arm based on motion sensors, characterized in that, The method specifically comprises the following steps: Step one: build and mechanical arm digital twin model, set the motion limit and initial position of each axis of the controlled mechanical arm; establish wireless communication between the digital twin mechanical arm and the real mechanical arm; Step two: establish wireless communication between the motion sensor and the computer; Step three: use Kalman filter for attitude solution, and solve the motion data of the motion sensor into the current attitude information; after obtaining the attitude information, the angle and displacement of each joint of the mechanical arm at the target attitude are calculated through the inverse kinematics algorithm of the mechanical arm; Step four: the motion sensor collects the three-axis acceleration, angular velocity and yaw angle parameters of the current hand motion, and converts them into the motion vector of the mechanical arm and the motion angle of each axis joint through the attitude solution algorithm and the inverse kinematics algorithm in the software; the motion parameters are sent to the controller of the mechanical arm through TCP communication to control the mechanical arm to move to the specified position, and the unity sends the motion of each life cycle to the digital twin model, so as to realize that the digital twin mechanical arm follows the real mechanical arm motion; Step three comprises the following steps, Step 3.1, the device that plays a function in the motion sensor is IMU sensor and magnetometer, the IMU sensor can measure the acceleration and angular velocity of xyz three-axis, and the magnetometer obtains the magnetic field information to obtain the current yaw angle; Step 3.2, initialize the state estimation and covariance matrix of Kalman filter; the state vector can be selected as [roll, pitch, yaw, gyro_bias_x, gyro_bias_y, gyro_bias_z], wherein roll and pitch represent the pitch and roll angles measured by the accelerometer, yaw represents the yaw angle measured by the magnetometer, gyro_bias_x, gyro_bias_y and gyro_bias_z represent the drift bias of the gyroscope; Step 3.3, define the measurement model to obtain the attitude information from the measurement data of the sensor; use the data of the accelerometer and the magnetometer to measure the attitude; the measurement model is represented as: ； ； ； wherein, , , is the acceleration of the xyz three axes obtained by the accelerometer in the IMU, , , is the magnetic field data of the xyz three axes obtained by the magnetometer measurement; , and is the attitude angle obtained by the accelerometer and the magnetometer measurement; Step 3.4, according to the system dynamic model, the angular velocity data of the gyroscope is used to predict the attitude state and covariance matrix at the next time; according to the measurement model, the measurement data of the accelerometer and the magnetometer are compared with the predicted value, and the predicted attitude state and covariance matrix are corrected; in each time period, the prediction and updating steps are repeated, and the new sensor data is used to update the attitude information constantly; Step 3.5, according to the geometric parameters of the mechanical arm, including the length of each joint, joint type, coordinate system definition; establish the forward kinematics model to convert the joint angle or displacement into the position and attitude of the end effector; Step 3.6, based on the forward kinematics model, the inverse kinematics model of the mechanical arm is established; the attitude and position of the end effector are converted into the joint angle; Step 3.7, the attitude information obtained after the attitude solution is substituted into the inverse kinematics model as the target attitude, and the angle and displacement of each joint of the mechanical arm at the target attitude are solved.

2. The motion sensor based digital twin robotic arm control method of claim 1, wherein, Step one comprises the following steps, Step 1.1, a three-dimensional model of the robot arm is established in the three-dimensional modeling software Solidworks and exported in the general.sld format; Step 1.2, the exported three-dimensional model in the.sld format is imported into the software for 3DMax format conversion and exported in the format recognized by unity, such as.fbx or.obj; Step 1.3, the file converted in the format is imported into unity, and the constraint relationship and parent-child relationship between each part are established; the joint rotation of each axis of the robot arm is normal, and there is no interference between them; Step 1.4, after building the digital twin model of the robot arm, relevant scripts are mounted in the unity scene, and function functions are mounted on the corresponding entity objects; the entity objects include ① rack: the base of the robot arm, used to fix the robot arm, the object is locked in the unity scene and cannot be moved; ② robot arm: the controlled object, accepts the motion instructions sent by unity to move; the function functions include: ① initialization function: automatically called each time after starting, initializes the position of each joint of the robot arm and initializes the communication parameters; ② robot arm motion limit function: mounted on each axis of the robot arm, once the robot arm motion exceeds its own limit or interference occurs, it will stop all motion and pop up an error message; ③ robot arm motion function: sends the motion vector of each axis of the robot arm to the robot arm model; the function is in the update function in the script, which will be executed once in each unity life cycle to ensure that the robot arm motion is updated in real time; Step 1.5, establish socket communication between the digital twin robot arm and the real robot arm, and the communication protocol is TCP / IP; the real robot arm is the server and the digital twin model is the client.

3. The motion sensor based digital twin robotic arm control method of claim 1, wherein, Step two includes the following steps, Step 2.1, the Wifi module is integrated in the motion sensor, and the IP address and port number of the Wifi module are configured through the computer; Step 2.2, the computer connects to the Wifi of the Wifi module, and sets the IP address and port number of the local computer to ensure that the IP address of the computer and the IP address of the Wifi module are in the same network segment; Step 2.3, establish socket communication between the motion sensor and the computer, and the communication protocol is TCP / IP; the computer is the server and the motion sensor is the client.

4. The motion sensor based digital twin robotic arm control method of claim 1, wherein, Step four includes the following steps, Step 4.1, start the motion sensor, press the gain button on the control handle to select the control gain, press the trigger key of the control handle to enter the motion control mode, each time the trigger key of the motion sensor is pressed, the current position is taken as the coordinate origin, the right is the positive direction of the X axis, the front is the positive direction of the Y axis, and the upward is the positive direction of the Z axis to establish a space coordinate system, and the motion data obtained are all based on the data in this space coordinate system; hold the motion sensor and move a distance, the IMU and magnetometer in the motion sensor obtain the current three-axis acceleration and angular velocity and yaw angle data; these data and the control gain are sent to the MCU, and the MCU uploads these data to the computer according to the communication protocol through Socket communication; Step 4.2, after receiving a data packet, the computer parses the motion data according to the communication protocol, and then obtains the current attitude information of the motion sensor through Kalman filtering algorithm for attitude solution; according to the control gain set by the control handle, the displacement vector in the attitude information is multiplied by the gain as the target position, and the angle rotation vector in the attitude information is unchanged; Step 4.3, the attitude information multiplied by the gain is taken as the target position to obtain the motion angle and displacement of each axis joint of the mechanical arm through inverse kinematics solution of the mechanical arm; Step 4.4, after obtaining the motion parameters of the joints of the mechanical arm, the computer uploads the motion information to the control board of the real mechanical arm through Socket communication; the control board controls the motor motion at each axis joint of the mechanical arm, which is the same as the motion controller. The mechanical arm establishes a space coordinate system with the current position as the coordinate origin, the right as the positive direction of X axis, the front as the positive direction of Y axis, and the upward as the positive direction of Z axis every time it starts to move; the motion of the mechanical arm is based on the coordinate system of the starting position; the displacement vector of the motion sensor in its own coordinate system multiplied by the motion gain obtains the displacement vector of the end of the mechanical arm in its coordinate system, so as to realize the replication of the human hand action; Step 4.5, at the same time of obtaining the motion parameters of the joints of the mechanical arm, the update function in the unity script program calls the motion parameters of the mechanical arm and executes the motion function of the virtual mechanical arm in each life cycle, controls the motion of the digital twin model to the specified position, and realizes the motion of the digital twin model following the real mechanical arm.

Images