A heterogeneous dual-arm end pose mapping method, system, device and storage medium

Abstract

The application discloses a kind of heterogeneous double mechanical arm end pose mapping method, system, device and storage medium, method includes the following steps: S1: obtaining the angular velocity of each joint of main mechanical arm, position and end attitude information, the spatial position and attitude of main mechanical arm end are calculated;S2: the motion trajectory data in the teaching process of main mechanical arm is collected, long short-term memory network model is established, the pose of future time step is predicted, and the motion trajectory of slave mechanical arm is planned;S3: data information is transferred, and communication delay problem is solved;S4: the pose information of main mechanical arm end is solved, joint angle is adjusted, and error is adjusted in combination with proportional-integral-derivative control;S5: position and velocity signal are filtered, and mechanical arm dynamics model is established, adaptive control law is designed, and trajectory smoothing algorithm is used;S6: virtual obstacle map is established, and safe working space is limited.The application realizes the comprehensive, accurate mapping and prediction of the end pose of heterogeneous double mechanical arm.

Description

Technical Field

[0001] This invention relates to the field of robotic arm technology, and more specifically, to a method, system, device, and storage medium for mapping the end effector pose of a heterogeneous dual robotic arm. Background Technology

[0002] With the continuous improvement of industrial automation, industrial robotic arms have been widely used in manufacturing. As a fundamental technology for industrial robotic arms, teaching programming directly affects their operational accuracy and work efficiency. A high-quality teaching system can not only significantly improve equipment production efficiency but also ensure production safety. However, because the teaching process involves complex spatial motion and precise control, it is often affected by various factors in practical applications, such as communication delays, environmental interference, and system nonlinearity. These interference factors may lead to decreased teaching accuracy or control failure, affecting not only product quality but also potentially causing major safety accidents. Therefore, achieving real-time monitoring and motion trajectory prediction during the teaching process is of great significance for ensuring the continuous and stable operation of the robotic arm and reducing safety risks.

[0003] Despite the enormous application potential of robotic arm teaching technology in industrial automation, it still faces numerous challenges in practical applications. Dai Guanghui, Zhang Qingqing, Dai Haoshuo, et al. proposed a robot autonomous learning and adaptive trajectory planning method for human-robot collaboration (Dai Guanghui, Zhang Qingqing, Dai Haoshuo, Xu Bing, Cao Zhuangzhi. A Robot Autonomous Learning and Adaptation Method for Human-Robot Collaboration [P]. Anhui: CN119077728A, 2024-12-06), introducing a dynamic motion primitive algorithm for learning and generalizing the robotic arm's motion trajectory. However, it may not be able to effectively predict more complex motion trajectories. On the other hand, Ling Yushi, Wang Hao, Zhang Guoyi, et al. proposed an analysis system for determining the causes of communication delays and its analysis. The method (Ling Yushi, Wang Hao, Zhang Guoyi, Chen Yongtao, Shi Liuyang, Liu Qi, Li Yanhong, Xiao Jian, Zhou Shijie, Wang Xuefeng, Cai Yanchun, Qin Qiqian, Zhang Qi, Zhou Wenqi, Kan Xiaocong, Guo Minghai, Zeng Yun, Zhu Weifeng. An analysis system and method for determining the causes of communication delay [P]. Guangdong: CN118921137A, 2024-11-08) generates a corresponding communication delay impact index by evaluating the communication performance of the device separately. However, for the communication of the robotic arm, there may be a situation where the processing speed is not fast enough, which leads to a certain error in the mapping between the master and slave robotic arms. Summary of the Invention

[0004] To address the aforementioned technical problems in existing technologies, this invention provides an innovative method, system, device, and storage medium for end-effector pose mapping of heterogeneous dual robotic arms. This invention integrates advanced technologies such as deep learning, model predictive control, and adaptive control. It acquires motion data from the master robotic arm through a teaching method, processes the data in real time, and maps it to the slave robotic arm to achieve precise pose following. During motion, the system continuously optimizes trajectory planning and dynamically adjusts control parameters to ensure the stability and safety of the collaborative motion of the two robotic arms. The system employs a multi-level control architecture, including modules for trajectory planning, motion prediction, pose mapping, and safety monitoring, achieving efficient collaborative control of the heterogeneous dual robotic arm system.

[0005] The technical problem addressed by this invention is:

[0006] The first aspect of this invention relates to a method for mapping the end effector pose of a heterogeneous dual robotic arm, comprising the following steps:

[0007] S1: Manually perform the main robotic arm teaching operation to obtain the angular velocity, position and end-effector attitude information of each joint of the main robotic arm, and calculate the spatial position and attitude of the end-effector of the main robotic arm.

[0008] S2: Collect motion trajectory data during the teaching process of the main robotic arm, establish a long short-term memory network model, predict the pose of the future time step, and plan the motion trajectory of the robotic arm.

[0009] S3: Data information is transmitted through the communication module, and model predictive control is used to solve the communication delay problem;

[0010] S4: Using proportional scaling and inverse kinematics, the pose information of the end effector of the main robotic arm is calculated, the joint angle is adjusted, and the error is adjusted by combining proportional-integral-derivative control.

[0011] S5: An extended Kalman filter is used to filter the position and velocity signals, a dynamic model of the robotic arm is established, an adaptive control law is designed, and a trajectory smoothing algorithm is adopted.

[0012] S6: Create a virtual obstacle map, define a safe workspace, detect potential collision risks in real time, plan obstacle avoidance trajectories, and set position and speed limits.

[0013] 2. The heterogeneous dual-arm end-effector pose mapping method according to claim 1, characterized in that step S1 specifically includes:

[0014] S11: The instructor manually performs the teaching actions on the main robotic arm;

[0015] S12: An absolute encoder is used to measure the absolute position information of each main robotic arm joint and obtain the rotation angle θ. The angular velocity ω and angular acceleration α of the main robotic arm can be obtained through the rotation angle θ.

[0016] S13: Store all collected data in a real-time data acquisition system and sample at fixed time intervals to form a series of time series data, which include the angle, angular velocity and acceleration values ​​of each joint;

[0017] S14: The spatial position and orientation of the end effector are obtained through forward kinematics calculations.

[0018] Furthermore, step S2 specifically includes:

[0019] S21: To analyze the teaching process of the main robotic arm, which may have regularities, deep learning is used.

[0020] S22: Collect motion trajectory sequence data {x1,x2,...,x} during the teaching process of the main robotic arm. T},in The pose vector at time t represents the position and orientation information, where T is the sequence length and d is the dimension of the pose vector.

[0021] S23: Construct an LSTM-based encoder network, perform state updates, and introduce an attention mechanism to compute the context vector c. t Then, a decoder network is constructed, which is combined with context vectors to predict future poses.

[0022] S24: Define the loss function for model training:

[0023]

[0024] L total =L pose +L reg (3)

[0025] Among them, y t The true pose is represented by θ, which represents all trainable parameters of the model, and λ is the regularization coefficient.

[0026] S25: Using the trained model, the input pose sequence of the main robotic arm is predicted in real time to obtain the pose prediction value for the future time step. Where k is the prediction step size;

[0027] S26: Based on the predicted pose sequence, plan the motion trajectory of the robotic arm. Where f plan Let τ be the trajectory planning function, and τ be the planned trajectory of the robotic arm. The prediction error is monitored in real time. When the error exceeds a preset threshold, online model fine-tuning is triggered.

[0028] Furthermore, step S3 specifically includes:

[0029] S31: Transform the pose signal data related to the main robotic arm that needs to be transmitted into a discrete-time state-space model of the system.

[0030] S32: Assume there is a time delay d in the information transmission system, that is, the control input u(k) is applied to the system at time step k, but its effect can only be observed at time step k+d;

[0031] S33: To handle time delay, the system's state vector is expanded to include past control inputs;

[0032] S34: Substituting into the MPC optimization problem, it is usually about minimizing the objective function within the prediction time domain N while satisfying system constraints; the MPC optimization problem considering time delay can be expressed as:

[0033]

[0034] subject to:

[0035]

[0036] u min ≤u(k+i|k)≤u max

[0037]

[0038] Where u(k+i|k) is the i-th control input predicted at time k, and y(k+i|k) is the i-th output predicted at time k. Let y be the i-th state predicted at time k. ref (k+i) is the reference output, Q and R are weight matrices used to penalize the output error and control the input, respectively, u min and u max These control the upper and lower limits of the input; x min and x max Represents the minimum and maximum limits of state variables;

[0039] S35: By incorporating the control inputs from the past d time steps into the state vector, MPC can consider the impact of time delay during the optimization process, thereby generating a better control input sequence and completing the signal transmission problem of the master and slave robotic arms.

[0040] Furthermore, step S4 specifically includes:

[0041] S41: To address the differences in size and workspace between the master and slave robotic arms, a scaling method is used to map the end-effector pose of the master robotic arm to the workspace of the slave robotic arm.

[0042] S42: By scaling the pose of the end effector of the main robotic arm, the target pose matrix T of the robotic arm is obtained. slave ;

[0043] S43: Based on the target pose matrix T slave The target angle of each joint is calculated step by step, and the slave robot arm rotates the corresponding angle to realize the mapping between the master and slave robot arms.

[0044] S44: In feedback control, PID control is used to correct the error between the master and slave robotic arms;

[0045] S45: The PID controller adjusts the control input u(t) in real time based on the current error e(t) to ensure that the slave robot can respond accurately to the real-time movement of the master robot.

[0046]

[0047] Among them, the proportional term K p It can quickly respond to errors, ensuring that the position can be adjusted immediately from the robotic arm, with the integral term K. i It helps to eliminate the static error of the system and avoid long-term error accumulation. The differential term K d This helps in predicting changes in error and reducing overshoot and oscillation.

[0048] Furthermore, step S5 specifically includes:

[0049] S51: Based on the current dimensions of the robotic arm, establish the dynamic model of the robotic arm and its system state-space equations;

[0050] S52: Perform the extended Kalman filter prediction step to predict the state. And prediction covariance update Perform the extended Kalman filter update step and calculate the Kalman gain. State estimation update Covariance matrix update P k|k =(IK k H k )P k|k-1 ;

[0051] Where the state vector Observation vector y k For position and velocity measurements, w k and v k Let w be the process noise and the measurement noise, respectively, and satisfy w.k ~N(0,Q) k ), v k ~N(0,R k ), Let be the Jacobian matrix of the state equation. Let be the Jacobian matrix of the observation equation;

[0052] S53: Design adaptive control law and adaptive term τ ad The renewal law;

[0053] S54: For system vibration, vibration characteristic identification and smoothing control are adopted;

[0054] S55: Calculate the system vibration energy and define the vibration index, when the system vibration energy V i Exceeding the preset threshold V {th} When the vibration is triggered, vibration suppression control is activated; for vibration systems, cubic spline interpolation is used to generate smooth trajectories for trajectory smoothing.

[0055] S56: To ensure that the pose mapping system of the robotic arm tends to be stable, a Lyapunov function is constructed to determine whether it is close to stability;

[0056]

[0057] Prove that its derivative satisfies:

[0058]

[0059] in, Let Γ be the position tracking error, Γ be the positive definite adaptive gain matrix, and K be the position tracking error. p and K d λ is a positive definite gain matrix. min (K d ) is a matrix K d The smallest eigenvalue.

[0060] Furthermore, step S6 specifically includes:

[0061] S61: Considering the real world, there may be situations where the robotic arm touches obstacles during its movement;

[0062] S62: Construct a 3D virtual obstacle map M for the robotic arm and establish a collision detection model;

[0063] S63: Define joint constraints for the safe workspace of the robotic arm for each joint's range of motion and speed limits, calculate the minimum distance from the current position of the robotic arm to the obstacle, and construct a safety index function based on the minimum distance. When the distance is less than the safety threshold, the function triggers obstacle avoidance, and a speed-constrained obstacle avoidance control law is designed.

[0064] S64: The system updates the safety status of the robotic arm mapping system in real time based on the set position and speed constraints.

[0065] A second aspect of the present invention relates to a heterogeneous dual-arm end-effector pose mapping system, comprising:

[0066] The pose acquisition module is responsible for acquiring the teaching operation data of the main robotic arm; it collects the angular velocity, position and end-effector posture information of each joint of the main robotic arm through an absolute encoder, and calculates the spatial position and posture parameters of the end-effector of the main robotic arm in real time by combining forward kinematics calculations.

[0067] The communication control module is used to transmit data information on the end-effector pose of the main robotic arm. It uses model predictive control to solve the problem of motion lag caused by communication delay, ensuring the real-time performance and accuracy of data transmission.

[0068] The pose mapping module is responsible for the pose calculation and control of the slave robot arm. It uses proportional scaling and inverse kinematics calculation to calculate the pose information of the end effector of the master robot arm, adjusts the joint angle, and combines proportional-integral-derivative control to adjust the error between the control output and the actual output in real time, so as to achieve precise pose mapping between the master and slave robot arms.

[0069] The trajectory prediction module is used to collect and analyze motion trajectory data during the teaching process of the main robotic arm; it uses a long short-term memory network to build a motion prediction model to predict the pose at future time steps and plan the motion trajectory of the robotic arm in advance; at the same time, it introduces an attention mechanism to extract key temporal features and continuously optimizes the prediction accuracy.

[0070] The motion optimization module is used to filter position and velocity signals; establish a dynamic model of the robotic arm to identify vibration characteristics; design an adaptive control law to compensate for system nonlinearity and external disturbances; and ensure the stability of the robotic arm's motion through a trajectory smoothing algorithm.

[0071] The safety control module is responsible for workspace constraints and safety management; it establishes a virtual obstacle map and defines a safe workspace, detects potential collision risks in real time, and automatically plans obstacle avoidance trajectories; it sets position and speed limit thresholds, monitors the system's operating status in real time, and ensures the reliable operation of the dual robotic arm system.

[0072] A third aspect of the present invention relates to a heterogeneous dual-arm end-effector pose mapping device, comprising a memory and one or more processors, wherein the memory stores executable code, and the one or more processors execute the executable code to implement the heterogeneous dual-arm end-effector pose mapping method described above.

[0073] A fourth aspect of the present invention relates to a computer-readable storage medium having a program stored thereon that, when executed by a processor, implements the heterogeneous dual-arm end-effector pose mapping method described above.

[0074] Compared with the prior art, the beneficial effects of the present invention are reflected in:

[0075] 1. This invention collects signals such as the angular velocity, position, and end-effector posture of the main robotic arm joints and calculates the spatial position using forward kinematics. Combined with model predictive control and proportional-integral-derivative control, it achieves comprehensive and accurate mapping and prediction of the end-effector posture of heterogeneous dual robotic arms.

[0076] 2. This invention records data such as predicted trajectory, real-time pose, and motion characteristics, and triggers corresponding obstacle avoidance mechanisms. When the collision detection model's judgment value reaches a certain threshold or an abnormal situation occurs, timely avoidance is performed, thereby improving the reliability and safety of the system.

[0077] 3. This invention makes full use of advanced prediction, control and optimization technologies, which significantly improves the performance of heterogeneous dual robotic arm systems in pose mapping and trajectory planning.

[0078] 4. This invention solves the technical problems of communication delay leading to motion lag and uneven motion trajectory in traditional heterogeneous dual-arm mapping systems, achieving real-time performance and accuracy; it also solves the technical problem of insufficient accuracy of existing robotic arm motion prediction models, making it difficult to accurately predict complex motion trajectories and resulting in unsatisfactory robotic arm following performance; it solves the technical problem of traditional pose mapping methods lacking adaptive capability, resulting in significant performance degradation when facing external disturbances and changes in system parameters; and it solves the technical problem of traditional control methods being unable to cope with nonlinear disturbances and workspace constraints, increasing the risk of collisions. Attached Figure Description

[0079] Figure 1 This is a flowchart illustrating a heterogeneous dual-arm end-effector pose mapping method and system in this embodiment;

[0080] Figure 2 This is a schematic diagram of the pose acquisition module for pose mapping of the end effector of the heterogeneous dual robotic arm of the present invention.

[0081] Figure 3This is a flowchart illustrating the communication control module for the end-effector pose mapping of the heterogeneous dual robotic arm of the present invention.

[0082] Figure 4 This is a schematic diagram of the pose mapping module for the end effector pose mapping of the heterogeneous dual robotic arm of the present invention.

[0083] Figure 5 This is a schematic diagram of the trajectory prediction module for the end-effector pose mapping of the heterogeneous dual robotic arm of the present invention.

[0084] Figure 6 This is a schematic diagram of the motion optimization module for end-effector pose mapping of the heterogeneous dual robotic arm of the present invention.

[0085] Figure 7 This is a schematic diagram of the safety control module for end-effector pose mapping of the heterogeneous dual robotic arm of the present invention.

[0086] Figure 8 This is a schematic diagram of the structure of a robotic arm prediction system in this embodiment;

[0087] Figure 9 This is a structural block diagram of the planning equipment of the present invention. Detailed Implementation

[0088] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the scope of the present invention.

[0089] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0090] The present invention will now be described in detail with reference to the accompanying drawings and exemplary embodiments.

[0091] Example 1

[0092] refer to Figures 1 to 7 The present invention provides a method for end-effector pose mapping of a heterogeneous dual robotic arm, comprising the following steps:

[0093] S1: The instructor manually performs a teaching operation on the main robotic arm. The absolute encoder acquires the angular velocity, position, and end-effector attitude information of each joint of the main robotic arm. The collected raw signals are summarized and calculated based on forward kinematics to deduce the spatial position and attitude of the end effector of the main robotic arm. The absolute encoder adopts high-precision multi-turn encoding technology, which has extremely high resolution and reliability. It can maintain the angle value in the event of power failure. Through the established DH parameter model, combined with the real-time motion parameters of each joint, the spatial attitude matrix of the end effector of the robotic arm relative to the base coordinate system is accurately calculated.

[0094] Step S1 specifically includes:

[0095] S11: The instructor manually performs the teaching actions on the main robotic arm;

[0096] S12: An absolute encoder is used to measure the absolute position information of each main robotic arm joint to obtain the rotation angle θ. The angular velocity and angular acceleration of the main robotic arm can be obtained from the rotation angle. The formulas for measuring angular velocity and angular acceleration are as follows:

[0097]

[0098] Where ω represents angular velocity, Δθ represents angular change, Δt represents time interval, and Δω is the change in angular velocity;

[0099] S13: Store all collected data in a real-time data acquisition system and sample at fixed time intervals to form a series of time-series data. These data include the angles, angular velocities, and acceleration values ​​of each joint;

[0100] S14: For a robotic arm with 6 degrees of freedom, the pose of the end effector can be described by a series of transformation matrices. Assuming each joint of the robotic arm is a rotary joint, the transformation matrix of each joint can be expressed as:

[0101]

[0102] Where, θ i It is the rotation angle of the i-th joint, α i It is the torsion angle of the joint (i.e., the angle between adjacent links), a i d is the link length of the i-th joint. i It is the offset of the i-th joint, and the transformation matrix T of each joint. i It describes the position and orientation of the joint in space;

[0103] For a robotic arm with 6 degrees of freedom, the total transformation matrix T of the end effector is... ee It is the product of all joint transformation matrices:

[0104] T ee =T1·T2·…·T6 (11)

[0105] Where T1, T2, ..., T6 are the transformation matrices of each joint;

[0106] Homogeneous transformation matrix T ee Composed of 4×4 matrices, representing the rotation and displacement information of the end effector:

[0107]

[0108] Among them, R ee The rotation matrix of the end effector represents its orientation and attitude, P ee The position vector of the end effector represents its spatial coordinates;

[0109] By calculating the forward kinematics, we can obtain the spatial position and orientation of the end effector;

[0110] Position can be determined by transformation matrix T ee Translation part P in ee get:

[0111]

[0112] Where (x,y,z) represents the coordinates in the three-dimensional coordinate system;

[0113] Rotation matrix R ee It can be represented as:

[0114]

[0115] Among them, R 11 ,R 12 ,…,R 33 R represents ee The value at the corresponding position in the matrix;

[0116] The Euler angles of the attitude are obtained through the rotation matrix R. ee Get:

[0117]

[0118] Where, θ x ,θ y ,θ z Represents Euler angles in the ZYX rotation order;

[0119] The quaternion of the attitude is obtained through the rotation matrix R ee Get:

[0120]

[0121] The quaternion is represented as q = (w, x, y, z).

[0122] S2: To achieve accurate prediction, deep learning is adopted to collect motion trajectory data during the teaching process of the main robotic arm. A motion prediction model is established using a Long Short-Term Memory (LSTM) network to predict the pose at future time steps and plan the motion trajectory of the robotic arm in advance. At the same time, key temporal features are extracted using an attention mechanism to optimize prediction accuracy. The LSTM network adopts a multi-layer stacked structure, containing 128 hidden layer neurons, and sets a sliding prediction window with 10 time steps. By introducing a multi-head self-attention mechanism, adaptive weighting of features at different time scales is achieved. Combined with cross-validation, the network hyperparameters are optimized, significantly improving the model's generalization ability and prediction accuracy.

[0123] Step S2 specifically includes:

[0124] S21: Based on the regularity characteristics of the main robotic arm's teaching process, deep learning methods are used to analyze the teaching data and collect motion trajectory sequences containing position and posture information;

[0125] S22: Collect motion trajectory sequence data {x1,x2,...,x} during the teaching process of the main robotic arm. T},in The pose vector at time t represents the position and orientation information, where T is the sequence length and d is the dimension of the pose vector.

[0126] S23: Construct an LSTM encoder network, and update the state of the memory cells through input gates, forget gates, and output gates to extract sequence features:

[0127] i t =σ(W i [h t-1 ,x t ]+b i (17)

[0128] f t =σ(W f [h t-1 ,x t ]+b f (18)

[0129] o t =σ(W o [h t-1 ,x t ]+b o (19)

[0130]

[0131] h t =o t ⊙tanh(ct ) (twenty two)

[0132] Among them, i t f t o t These are the input gate, forget gate, and output gate, respectively. t h represents the state of a memory unit. t For hidden output, W i W f W o W c Let b be the weight matrix. i b f b o b c It is the bias vector;

[0133] An attention mechanism is introduced to enhance the model's ability to perceive key temporal features by calculating attention scores, weights, and context vectors.

[0134] First, calculate the attention score:

[0135]

[0136] Then calculate the attention weights:

[0137]

[0138] Finally, the context vector is obtained:

[0139]

[0140] Among them, v a W represents the attention vector parameters. a Let s be the attention weight matrix. {j-1} This represents the decoder's state at the previous moment;

[0141] Construct a decoder network and combine it with the context vector obtained through the attention mechanism to predict the pose information at future time steps:

[0142] s t =f LSTM (y t-1 ,[c t ;s t-1 (26)

[0143]

[0144] Among them, s t Decoder status. For the predicted pose output, W p and b p These are the parameters for the prediction layer;

[0145] S24: Design a loss function that includes a prediction error term and a regularization term for model training optimization:

[0146]

[0147] L total =L pose +L reg (3)

[0148] Among them, y t The true pose is represented by θ, which represents all trainable parameters of the model, and λ is the regularization coefficient.

[0149] S25: Utilize the trained model to predict the pose sequence of the main robotic arm in real time, and obtain the pose prediction value of the future predicted step length. Where k is the prediction step size;

[0150] S26: Based on the predicted pose sequence, the motion trajectory of the robotic arm is generated through a trajectory planning function:

[0151]

[0152] Where f plan Let τ be the trajectory planning function, and τ be the planned trajectory of the robotic arm.

[0153] Real-time monitoring of prediction errors When the error exceeds a preset threshold, prediction accuracy is maintained through online fine-tuning:

[0154]

[0155] Where η is the learning rate, ensuring that the prediction accuracy always meets the system requirements.

[0156] S3: Based on the end-effector pose of the main robotic arm, the communication module transmits data information and uses Model Predictive Control (MPC) to address motion lag caused by communication delay. The MPC establishes a dynamic prediction model of the system and continuously optimizes the control sequence over a future period online. The prediction time domain is set to 500ms, and the control time domain is 100ms. The optimal control quantity is solved using a quadratic programming algorithm, thereby effectively compensating for system response lag caused by communication delay.

[0157] Step S3 specifically includes:

[0158] S31: Transform the signal data related to the main robotic arm that needs to be transmitted into a discrete-time state-space model of the system;

[0159] x(k+1)= A x(k)+ B u(k) (30)

[0160] y(k)= C x(k) (31)

[0161] Where x(k) is the state vector of the system at time step k, u(k) is the control input vector of the system at time step k, y(k) is the output vector of the system at time step k, and A, B, and C are the state matrix, input matrix, and output matrix of the system, respectively.

[0162] S32: Assume there is a time delay d in the information transmission system, that is, the control input u(k) is applied to the system at time step k, but its effect can only be observed at time step k+d;

[0163] S33: To handle time delays, the system's state vector can be extended to include past control inputs. Define the extended state vector. for:

[0164]

[0165] Where u(k-1), u(k-2), ..., u(kd) are the control inputs of the past d time steps;

[0166] The extended state-space model is as follows:

[0167]

[0168] in, It is the expanded state matrix. It is the expanded input matrix. It is the expanded output matrix.

[0169] The specific form is:

[0170]

[0171]

[0172] S34: Substituting into the MPC optimization problem, the objective function is typically minimized within the prediction time domain N, while simultaneously satisfying system constraints. The MPC optimization problem considering time delay can be expressed as:

[0173]

[0174] subject to:

[0175]

[0176] u min ≤u(k+i|k)≤u max

[0177] xmin ≤x(k+i|k)≤x max (4)

[0179] Where: y ref (k+i) is the reference output, Q and R are weight matrices used to penalize the output error and control the input, respectively, u min and u max These control the upper and lower limits of the input;

[0180] S35: By incorporating the control inputs from the past d time steps into the state vector, MPC can consider the impact of time delay during the optimization process, thereby generating a better control input sequence and completing the signal transmission problem of the master and slave robotic arms.

[0181] S4: The slave robot uses proportional scaling and inverse kinematics to calculate the pose information of the end effector of the master robot, adjusts the joint angles, and then combines proportional-integral-derivative control to adjust the error between the control output and the actual output, thereby realizing the mapping of the end effector pose of the master robot. The PID controller adopts an adaptive parameter adjustment strategy, dynamically adjusting the three gain parameters of proportional, integral and derivative according to the real-time operating status of the system. It solves the inverse kinematic equation through the Jacobian matrix and uses the gradient descent method to iteratively optimize the joint angle solution, ensuring that the slave robot can accurately follow the motion trajectory of the master robot.

[0182] Step S4 specifically includes:

[0183] S41: To address the differences in size and workspace between the master and slave robotic arms, a proportional scaling method is used to achieve pose mapping;

[0184] S42: Description of the end-effector pose of a 6-DOF master robotic arm. The end-effector pose is described using a homogeneous transformation matrix T. master express:

[0185]

[0186] Among them, R master This represents the rotation matrix at the end of the main robotic arm. Indicates the position of the end effector of the main robotic arm;

[0187] By introducing a scaling factor that reflects the ratio of the master and slave robot arm lengths, the target pose matrix T of the slave robot arm is calculated. slave :

[0188]

[0189] in, This represents the scaling factor, reflecting the proportional relationship between the master and slave robotic arm lengths;

[0190] S43: Based on the target pose matrix T slave The inverse kinematics method is used to sequentially solve for the target angles from each joint of the robotic arm:

[0191] First joint angle:

[0192]

[0193] The angles of the second and third joints:

[0194]

[0195] Angles of the fourth, fifth, and sixth joints:

[0196]

[0197] Control the robotic arm to perform corresponding joint angle movements, thus completing the master-slave mapping;

[0198] S44: In feedback control, PID control is used to correct the error between the master and slave robotic arms;

[0199] S45: The PID controller can adjust the control input u(t) in real time according to the current error e(t) to ensure that the slave robot can make accurate responses based on the real-time movement of the master robot.

[0200]

[0201] Among them, the proportional term K p It can quickly respond to errors, ensuring that the position can be adjusted immediately from the robotic arm, with the integral term K. i This helps to eliminate the static error of the system and avoid long-term error accumulation. The differential term K d This helps in predicting changes in error and reducing overshoot and oscillation.

[0202] S5: An extended Kalman filter is used to filter the position and velocity signals, establish a dynamic model of the robotic arm, realize vibration feature identification, design an adaptive control law to compensate for system nonlinearity and external disturbances, and use a trajectory smoothing algorithm to ensure the stability of the robotic arm's motion. The extended Kalman filter adopts an adaptive noise covariance matrix design, dynamically adjusts the reliability of the system state estimation by evaluating the quality of measurement data online, and constructs an accurate dynamic model that considers factors such as joint friction, Coriolis force and gravity. Combined with the fifth-order polynomial interpolation method, trajectory smoothing is achieved, effectively suppressing the vibration and shaking phenomena of the robotic arm during high-speed motion.

[0203] Step S5 specifically includes:

[0204] S51: Based on the actual physical dimensions of the robotic arm, establish a dynamic model including the inertia matrix, Coriolis force and centrifugal force matrices, and gravity matrix:

[0205]

[0206] Among them, q, Let M(q) be the joint position, velocity, and acceleration vectors, respectively, and M(q) be the inertia matrix. Let G(q) be the Coriolis force and centrifugal force matrix, G(q) be the gravity matrix, τ be the control input, and d(t) be the external disturbance.

[0207] The dynamic model is transformed into state-space equations, defining state vectors and observation vectors, and considering process noise and measurement noise:

[0208] x k+1 =f(x) k ,u k )+w k (46)

[0209] y k =h(x k )+v k (47)

[0210] Where the state vector Observation vector y k For position and velocity measurements, w k and v k Let w be the process noise and the measurement noise, respectively, and satisfy w. k ~N(0,Q) k ), v k ~N(0,R k );

[0211] S52: Perform extended Kalman filter predictions, including state prediction and prediction covariance update, which involves the Jacobian matrix of the state equation:

[0212] State prediction:

[0213]

[0214] Predicting covariance updates:

[0215]

[0216] in Let be the Jacobian matrix of the state equation;

[0217] Perform extended Kalman filter updates, calculate the Kalman gain, and update the state estimate and covariance matrix:

[0218] Calculate the Kalman gain:

[0219]

[0220] State estimation update:

[0221]

[0222] Covariance matrix update:

[0223] P k|k =(IK k H k )P k|k-1 (52)

[0224] in Let be the Jacobian matrix of the observation equation;

[0225] S53: Design an adaptive control law that includes estimates of dynamic parameters, position tracking error, and a positive definite gain matrix.

[0226]

[0227] in, These are estimated values ​​of the system's dynamic parameters. K represents the position tracking error. p and K d It is a positive definite gain matrix;

[0228] Define an update law for the adaptive term to compensate for system nonlinearity and external disturbances:

[0229]

[0230] Where Γ is a positive definite adaptive gain matrix, used to compensate for system nonlinearity and external disturbances;

[0231] S54: For system vibration, vibration characteristic identification and smoothing control are adopted:

[0232] Calculate the vibration energy of the system:

[0233]

[0234] Define vibration index:

[0235]

[0236] When V i Exceeding the preset threshold V {th} When this occurs, vibration suppression control is triggered;

[0237] For vibration systems, cubic spline interpolation is used to generate smooth trajectories, and trajectory smoothing is then performed.

[0238] q d (t)=a0+a1t+a2t 2 +a3t 3 (57)

[0239]

[0240] Among them, the coefficients a0, a1, a2, and a3 are solved by boundary conditions to obtain q. d (t0)=q0,q d (t f )=q f ,

[0241] S55: To ensure the robot arm's pose mapping system tends to stabilize, a Lyapunov function is constructed to determine whether it is approaching stability:

[0242]

[0243] Prove that its derivative satisfies:

[0244]

[0245] Where, λ min (K d ) is a matrix K d The smallest eigenvalue.

[0246] S6: To address workspace constraints and safety control, a virtual obstacle map is established to define a safe workspace, and potential collision risks are detected in real time. The system automatically plans obstacle avoidance trajectories, sets position and speed limits, and ensures reliable system operation.

[0247] Step S6 specifically includes:

[0248] S61: Consider the potential collisions between the robotic arm and obstacles during actual movement;

[0249] S62: Construct a 3D virtual obstacle map of the robotic arm's motion environment, and mark the obstacle region and free space in a binarized form:

[0250]

[0251] Where (x,y,z) represents a three-dimensional coordinate point in the workspace, obstacle areas are marked as 1, free space is marked as 0, and map resolution is set to δ;

[0252] S63: Define the angle and angular velocity constraints for each joint of the robotic arm to ensure that the robotic arm moves within a safe workspace:

[0253]

[0254] Where, q i and Let represent the angle and angular velocity of the i-th joint, respectively, and n be the number of joints in the robotic arm. Each joint has its own limits on its range of motion and speed.

[0255] Calculate the minimum Euclidean distance between the robotic arm and the obstacle based on the current configuration of the robotic arm:

[0256]

[0257] Where R(q) represents the set of spatial points under the current configuration of the robotic arm, O is the set of obstacle points, and ∥·∥2 represents the Euclidean distance;

[0258] Design a safety metric function based on minimum distance, and activate the obstacle avoidance mechanism when the distance is less than a preset safety threshold:

[0259]

[0260] Where, d safe The safe distance threshold is α, and the attenuation coefficient is α. This function triggers obstacle avoidance when the distance is less than the safe distance threshold.

[0261] Design a velocity-constrained obstacle avoidance control law, generating obstacle avoidance velocity components through the gradient of the safety index function:

[0262]

[0263] Among them, v d For the desired speed, k v To improve obstacle avoidance, The gradient of the safety index function is used to generate the obstacle avoidance velocity components;

[0264] S64: Update the safety status of the robotic arm mapping system in real time, and set the limit constraints for position and speed.

[0265]

[0266] Set the position and velocity limits as follows:

[0267]

[0268] Where, p base R represents the position of the robotic arm base. max For the maximum working radius, v max This is the maximum permissible speed.

[0269] Example 2

[0270] refer to Figure 8The present invention provides a heterogeneous dual-arm end-effector pose mapping system, comprising:

[0271] The pose acquisition module is responsible for acquiring the teaching operation data of the main robotic arm; it collects the angular velocity, position and end-effector posture information of each joint of the main robotic arm through an absolute encoder, and calculates the spatial position and posture parameters of the end-effector of the main robotic arm in real time by combining forward kinematics calculations.

[0272] The communication control module is used to transmit data information on the end-effector pose of the main robotic arm. It uses model predictive control to solve the problem of motion lag caused by communication delay, ensuring the real-time performance and accuracy of data transmission.

[0273] The pose mapping module is responsible for the pose calculation and control of the slave robot arm. It uses proportional scaling and inverse kinematics calculation to calculate the pose information of the end effector of the master robot arm, adjusts the joint angle, and combines proportional-integral-derivative control to adjust the error between the control output and the actual output in real time, so as to achieve precise pose mapping between the master and slave robot arms.

[0274] The trajectory prediction module is used to collect and analyze motion trajectory data during the teaching process of the main robotic arm; it uses a long short-term memory network to build a motion prediction model to predict the pose at future time steps and plan the motion trajectory of the robotic arm in advance; at the same time, it introduces an attention mechanism to extract key temporal features and continuously optimizes the prediction accuracy.

[0275] The motion optimization module is used to filter position and velocity signals; establish a dynamic model of the robotic arm to identify vibration characteristics; design an adaptive control law to compensate for system nonlinearity and external disturbances; and ensure the stability of the robotic arm's motion through a trajectory smoothing algorithm.

[0276] The safety control module is responsible for workspace constraints and safety management; it establishes a virtual obstacle map and defines a safe workspace, detects potential collision risks in real time, and automatically plans obstacle avoidance trajectories; it sets position and speed limit thresholds, monitors the system's operating status in real time, and ensures the reliable operation of the dual robotic arm system.

[0277] Example 3

[0278] The present invention provides a heterogeneous dual-arm end-effector pose mapping device, including a memory and one or more processors. The memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the heterogeneous dual-arm end-effector pose mapping method described above.

[0279] Example 4

[0280] The present invention provides a computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements the above-described heterogeneous dual robotic arm end-effector pose mapping method.

[0281] The present invention also provides Figure 9 The one shown corresponds to Figure 1 A schematic structural diagram of the planning device. (See attached diagram.) Figure 9 At the hardware level, the planned equipment includes a processor, internal bus, network interface, memory, and non-volatile memory, and may also include other hardware required for the business operations. The processor reads the corresponding computer program from the non-volatile memory into memory and then runs it to achieve the above. Figure 1 The data acquisition method described above. Of course, in addition to software implementation, this invention does not exclude other implementation methods, such as logic devices or a combination of hardware and software, etc. That is to say, the execution subject of the following processing flow is not limited to each logic unit, but can also be hardware or logic devices.

[0282] The above description is merely an embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principle of the present invention should be included within the scope of the claims of the present invention.

Claims

1. A method for mapping the end effector pose of a heterogeneous dual robotic arm, characterized in that, Includes the following steps: S1: Manually perform teaching operations on the main robotic arm to obtain the angular velocity, position and end-effector attitude information of each joint of the main robotic arm, and calculate the spatial position and attitude of the end-effector of the main robotic arm. S2: Collect motion trajectory data of the main robotic arm during the teaching process, establish a long short-term memory network model, predict the pose of the future time step, and plan the motion trajectory of the robotic arm; S3: Data information is transmitted through the communication module, and model predictive control is used to solve the communication latency problem; S4: Using proportional scaling and inverse kinematics, the pose information of the end effector of the main robotic arm is calculated, the joint angles are adjusted, and the error is adjusted by combining proportional-integral-derivative control. S5: An extended Kalman filter is used to filter the position and velocity signals, a dynamic model of the robotic arm is established, an adaptive control law is designed, and a trajectory smoothing algorithm is adopted. S6: Create a virtual obstacle map, define a safe workspace, detect potential collision risks in real time, plan obstacle avoidance trajectories, and set position and speed limits; Step S1 specifically includes: S11: The instructor manually performs the teaching actions on the main robotic arm; S12: Employs an absolute encoder to measure the absolute position information of each main robotic arm joint and obtain the rotation angle. angular velocity of the main robotic arm With angular acceleration By rotating the angle get; S13: Store all collected data in a real-time data acquisition system and sample at fixed time intervals to form a series of time-series data, which includes the angle, angular velocity and acceleration values ​​of each joint; S14: The spatial position and orientation of the end effector are obtained through forward kinematics calculations; Step S2 specifically includes: S21: To analyze the teaching process of the main robotic arm, which may exhibit regularities, deep learning is employed. S22: Collect motion trajectory sequence data during the teaching process of the main robotic arm. ,in Let d represent the pose vector at time t, containing position and orientation information, where T is the sequence length and d is the dimension of the pose vector; S23: Construct an LSTM-based encoder network, perform state updates, and introduce an attention mechanism to compute the context vector. Then, a decoder network is constructed, which is combined with context vectors to predict future poses. ; S24: Define the loss function for model training: （1） （2） （3） in, For the true pose, For all trainable parameters of the model, The regularization coefficient; S25: Using the trained model, the input master robotic arm pose sequence is predicted in real time to obtain the pose prediction value for the future time step. ,in To predict the step size; S26: Based on the predicted pose sequence, plan the motion trajectory of the robotic arm. ,in For trajectory planning function, To plan the trajectory of the robotic arm and monitor the prediction error in real time. When the error exceeds the preset threshold At that time, online model fine-tuning is triggered; Step S3 specifically includes: S31: Transform the pose signal data related to the main robotic arm that needs to be transmitted into a discrete-time state-space model of the system; S32: Assume there is a time delay in the information transmission system. That is, control input At time step Apply to the system, but at time step Only then can its effects be observed; S33: To handle time delay, the system's state vector is expanded to include past control inputs; S34: Substituting into the MPC optimization problem typically occurs in the prediction time domain. The internal minimization objective function, while satisfying system constraints, is represented as the MPC optimization problem considering time delay: （4） in: In time k The first time predicted i One control input, In time k The first time predicted i One output, In time k The first time predicted i One state, This is the reference output. and These are weight matrices, used to penalize output error and control input, respectively. and These control the upper and lower limits of the input; and Represents the minimum and maximum limits of state variables; S35: By passing through the past The control inputs at each time step are incorporated into the state vector. MPC considers the impact of time delay during the optimization process, thereby generating a better control input sequence and completing the signal transmission problem of the master and slave robotic arms. Step S4 specifically includes: S41: To address the differences in size and workspace between the master and slave robotic arms, a scaling method is used to map the end-effector pose of the master robotic arm to the workspace of the slave robotic arm. S42: By scaling the pose of the end effector of the main robotic arm, the target pose matrix of the robotic arm is obtained. ; S43: Based on the target pose matrix The target angle of each joint is calculated step by step, and the driven robotic arm rotates the corresponding angle to achieve master-slave robotic arm mapping; S44: In feedback control, PID control is used to correct the error between the master and slave robotic arms; S45: The PID controller adjusts the current error... Real-time adjustment of control input This ensures that the slave robot can respond accurately based on the real-time movement of the master robot. （5） Among them, the proportional term Capable of rapid error response, ensuring immediate position adjustment from the robotic arm, integral term It helps to eliminate the static error of the system and avoid long-term error accumulation. Differential terms This helps to predict changes in error and reduce overshoot and oscillation; Step S5 specifically includes: S51: Based on the current dimensions of the robotic arm, establish the dynamic model of the robotic arm and its system state-space equations; S52: Perform the extended Kalman filter prediction step to predict the state. And prediction covariance update Perform the extended Kalman filter update step and calculate the Kalman gain. State estimation update Covariance matrix update ; Where the state vector Observation vector These are position and velocity measurements. and They are process noise and measurement noise, respectively, and satisfy the following conditions: , , Let be the Jacobian matrix of the state equation. The Jacobian matrix of the observation equation; S53: Design adaptive control laws and adaptive terms The renewal law; S54: For system vibration, vibration characteristic identification and smoothing control are adopted; S55: Calculate the system vibration energy and define the vibration index. When the system vibration energy... Exceeding the preset threshold When the vibration is triggered, vibration suppression control is activated; for vibration systems, cubic spline interpolation is used to generate smooth trajectories for trajectory smoothing. S56: To ensure that the pose mapping system of the robotic arm tends to be stable, a Lyapunov function is constructed to determine whether it is close to stability; （6） Prove that its derivative satisfies: （7） in, For position tracking error, It is a positive definite adaptive gain matrix. and It is a positive definite gain matrix; For matrix The smallest eigenvalue.

2. The method for mapping the end effector pose of a heterogeneous dual robotic arm according to claim 1, characterized in that, Step S6 specifically includes: S61: Considering the real world, the robotic arm may encounter obstacles during its movement; S62: Construct a 3D virtual obstacle map M for the robotic arm and establish a collision detection model; S63: Define joint constraints for the safe workspace of the robotic arm by setting limits for the range of motion and speed of each joint, calculate the minimum distance from the current position of the robotic arm to the obstacle, and construct a safety index function based on the minimum distance. When the distance is less than the safety threshold, obstacle avoidance is triggered, and an obstacle avoidance control law under speed constraints is designed. S64: The system updates the safety status of the robotic arm mapping system in real time based on the set position and speed constraints.

3. A mapping system for implementing the heterogeneous dual-arm end-effector pose mapping method as described in claim 1, characterized in that, include: The pose acquisition module is responsible for acquiring the teaching operation data of the main robotic arm; The angular velocity, position, and end-effector attitude information of each joint of the main robotic arm are collected by an absolute encoder. Combined with forward kinematics calculations, the spatial position and attitude parameters of the end-effector of the main robotic arm are calculated in real time. The communication control module is used to transmit data information on the end effector posture of the main robotic arm. It employs model predictive control to address the motion lag caused by communication delays, ensuring the real-time performance and accuracy of data transmission. The pose mapping module is responsible for the pose calculation and control of the slave robot arm. It uses proportional scaling and inverse kinematics calculations to calculate the pose information of the master robot arm's end effector, adjusts joint angles, and combines proportional-integral-derivative control to adjust the error between the control output and the actual output in real time, achieving precise pose mapping between the master and slave robot arms. The trajectory prediction module collects and analyzes motion trajectory data during the teaching process of the main robotic arm; it uses a long short-term memory network to build a motion prediction model to predict the pose at future time steps and plan the robotic arm's motion trajectory in advance; at the same time, it introduces an attention mechanism to extract key temporal features and continuously optimizes the prediction accuracy. The motion optimization module is used to filter position and velocity signals; A dynamic model of the robotic arm is established to identify vibration characteristics, and an adaptive control law is designed to compensate for system nonlinearity and external disturbances. A trajectory smoothing algorithm is used to ensure the stability of the robotic arm's motion. The safety control module is responsible for workspace constraints and safety management; it establishes a virtual obstacle map and defines a safe workspace, detects potential collision risks in real time, and automatically plans obstacle avoidance trajectories; it sets position and speed limit thresholds, monitors the system's operating status in real time, and ensures the reliable operation of the dual robotic arm system.

4. A heterogeneous dual-arm end-effector pose mapping device, characterized in that, The device includes a memory and one or more processors, wherein the memory stores executable code, and the one or more processors execute the executable code to implement the heterogeneous dual-arm end-effector pose mapping method according to any one of claims 1-2.

5. A computer-readable storage medium, characterized in that, It stores a program that, when executed by a processor, implements the heterogeneous dual-arm end-effector pose mapping method as described in any one of claims 1-2.

Images