Heterogeneous double-mechanical-arm tail end pose mapping method, system and device and storage medium

Abstract

The invention discloses a heterogeneous double-mechanical-arm tail end pose mapping method, system and device and a storage medium, and the method comprises the following steps: S1, obtaining the angular velocity, position and tail end pose information of each joint of a main mechanical arm, and calculating the spatial position and pose of the tail end of the main mechanical arm; s2, motion trail data in the teaching process of the main mechanical arm is collected, a long-short-term memory network model is established, the pose of the future time step length is predicted, and the motion trail of the auxiliary mechanical arm is planned; s3, data information is transmitted, and the problem of communication delay is solved; s4, the pose information of the tail end of the main mechanical arm is solved, the joint angle is adjusted, and the error is adjusted in combination with proportional-integral-differential control; s5, the position and speed signals are filtered, a mechanical arm dynamic model is established, an adaptive control law is designed, and a trajectory smoothing algorithm is adopted; and S6, establishing a virtual obstacle map, and limiting a safe working space. According to the method, comprehensive and accurate mapping and prediction of the tail end poses of the heterogeneous double mechanical arms are achieved.

Description

Technical Field

[0001] The present invention relates to the field of robotic arm technology, and more specifically, to a heterogeneous dual robotic arm end-point posture mapping method, system, device and storage medium. Background Art

[0002] With the continuous improvement of the level of industrial automation, industrial robotic arms have been widely used in the field of production and manufacturing. As the basic technology of industrial robotic arms, teaching programming directly affects the operating accuracy and work efficiency of the robotic arms. A high-quality teaching system can not only significantly improve the production efficiency of the equipment, but also ensure production safety. However, since the teaching process involves complex spatial motion and precise control, it is often affected by multiple factors such as communication delays, environmental interference, and system nonlinearity in actual applications. These interference factors may cause the teaching accuracy to decrease or the control to fail, which not only affects the product quality, but may also cause major safety accidents. Therefore, it is of great significance to realize real-time monitoring of the teaching process and motion trajectory prediction to ensure the continuous and stable operation of the robotic arm and reduce safety risks.

[0003] Although the robot arm teaching technology has shown great application potential in the field of industrial automation, it still faces many challenges in practical application. Dai Guanghui, Zhang Qingqing, Dai Haoshuo, etc. proposed a robot autonomous learning and adaptive trajectory planning method for human-machine collaboration (Dai Guanghui, Zhang Qingqing, Dai Haoshuo, Xu Bing, Cao Zhuangzhi. A robot autonomous learning and adaptation for human-machine collaboration [P]. Anhui: CN119077728A, 2024-12-06), introducing a dynamic motion primitive algorithm to learn and generalize the robot arm motion trajectory, but it may not be able to effectively predict when facing more complex motion trajectories; on the other hand, Ling Yushi, Wang Hao, Zhang Guoyi, etc. proposed an analysis system for determining the cause of communication delay and its analysis 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 analysis method for determining the cause of communication delay [P]. Guangdong: CN118921137A, 2024-11-08), by evaluating the communication performance of the equipment separately, the corresponding communication delay impact index is generated, but for the communication of the robot arm, the processing speed may not be fast enough, which leads to certain errors in the mapping of the master and slave robot arms. Summary of the invention

[0004] In order to solve the above technical problems existing in the prior art, the present invention provides an innovative heterogeneous dual-arm terminal posture mapping method, system, device and storage medium. The present invention integrates advanced technologies such as deep learning, model predictive control, and adaptive control. The motion data of the master robot is obtained by teaching. The system processes and maps it to the slave robot in real time to achieve accurate posture following. During the movement, the system continuously optimizes the trajectory planning and dynamically adjusts the control parameters to ensure the stability and safety of the coordinated movement of the dual robot arms. The system of the present invention adopts a multi-level control architecture, including modules such as trajectory planning, motion prediction, posture mapping, and safety monitoring, to achieve efficient collaborative control of the heterogeneous dual robot arm system.

[0005] The technical problems adopted by the present invention are:

[0006] A first aspect of the present invention relates to a heterogeneous dual-manipulator end-point pose mapping method, comprising the following steps:

[0007] S1: Manually perform the teaching operation of the main robot arm, obtain the angular velocity, position and end posture information of each joint of the main robot arm, and calculate the spatial position and posture of the end of the main robot arm;

[0008] S2: Collect the motion trajectory data of the master robot during the teaching process, establish a long short-term memory network model, predict the position and posture of the future time step, and plan the motion trajectory of the slave robot;

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

[0010] S4: The slave robot uses scaling and inverse kinematics to solve the posture information of the end of the master robot, adjust the joint angle, and adjust the error in combination with proportional-integral-differential control;

[0011] S5: Use extended Kalman filter to filter the position and velocity signals, establish the dynamic model of the robot arm, design the adaptive control law, and use the trajectory smoothing algorithm;

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

[0013] 2. A heterogeneous dual-manipulator end-point pose mapping method according to claim 1, characterized in that step S1 specifically comprises:

[0014] S11: The instructor manually performs a teaching action on the main robot arm;

[0015] S12: using an absolute encoder to measure the absolute position information of each main robot arm joint to obtain a rotation angle θ, the main robot arm angular velocity ω and angular acceleration α can be obtained through the rotation angle θ;

[0016] S13: All collected data are stored in a real-time data acquisition system and sampled at fixed time intervals to form a series of time series data, which include angles, angular velocities and acceleration values ​​of each joint;

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

[0018] Furthermore, step S2 specifically includes:

[0019] S21: Based on the possible regularity of the teaching process of the main robot arm, deep learning is used to analyze its teaching process;

[0020] S22: Collect the motion trajectory sequence data {x 1 ,x 2 ,...,x T},in Represents the pose vector at time t, which contains position and pose information, T is the sequence length, and d is the pose vector dimension;

[0021] S23: Build an LSTM-based encoder network, update the state, and introduce the attention mechanism to calculate the context vector c t , then build the decoder network and combine the context vector to predict the future pose

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

[0023]

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

[0025] Among them, y t is the true pose, θ is all the trainable parameters of the model, and λ is the regularization coefficient;

[0026] S25: Use the trained model to make real-time predictions on the input main robot arm posture sequence to obtain the posture prediction value of the future time step Where k is the prediction step length;

[0027] S26: Based on the predicted pose sequence, plan the motion trajectory of the slave robot where f plan is the trajectory planning function, and τ is the trajectory of the slave robot obtained by planning. And the prediction error is monitored in real time When the error exceeds the preset threshold ∈, the model is triggered to be fine-tuned online.

[0028] Furthermore, step S3 specifically includes:

[0029] S31: converting the posture signal data related to the main robot arm to be transmitted into a discrete time state space model of the system;

[0030] S32: Assume that there is a time delay d in the information transfer 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 the delay, the state vector of the system is expanded to include the state vector of the past control input;

[0032] S34: Substituting into the MPC optimization problem is usually to minimize the objective function in the prediction time domain N while satisfying the system constraints; the MPC optimization problem considering the delay can be expressed as:

[0033]

[0034] subject to:

[0035]

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

[0037]

[0038] Among them, 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. is the i-th state predicted at time k, y ref (k+i) is the reference output, Q and R are weight matrices used to penalize output error and control input, respectively, u min and u max are the upper and lower limits of the control input; x min and x max Indicates minimum and maximum limits on state variables;

[0039] S35: By incorporating the control inputs of the past d time steps into the state vector, MPC can consider the impact of time delay in the optimization process, thereby generating a more optimal control input sequence and solving the signal transmission problem of the master-slave manipulator.

[0040] Furthermore, step S4 specifically includes:

[0041] S41: In view of the differences in size and workspace between the master and slave manipulators, the end position of the master manipulator is mapped to the workspace of the slave manipulator by scaling;

[0042] S42: By scaling the posture ratio of the end of the main robot arm, the target posture matrix T of the slave robot arm is obtained. slave ;

[0043] S43: According to 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 achieve the master-slave robot arm mapping;

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

[0045] S45: The PID controller adjusts the control input u(t) in real time according to the current error e(t) to ensure that the slave robot can make an accurate response according to the real-time motion status of the master robot:

[0046]

[0047] Among them, the proportional term K p It can respond quickly to errors, ensuring that the slave arm can adjust its position immediately. 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 predict changes in errors and reduce overshoot and oscillation.

[0048] Furthermore, step S5 specifically includes:

[0049] S51: Establishing a dynamic model of the robot arm and its system state space equation according to the current robot arm related dimensions;

[0050] S52: Execute the extended Kalman filter prediction step to perform state prediction and forecast covariance update Perform the extended Kalman filter update step and calculate the Kalman gain State Estimate Update Covariance matrix update P k|k =(IK k H k ) k|k-1 ;

[0051] The state vector Observation vector y k is the position and velocity measurement, w k and v k are process noise and measurement noise respectively, and satisfy wk ~N(0,Q k ), v k ~N(0,R k ), is the Jacobian matrix of the state equation, is the Jacobian matrix of the observation equation;

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

[0053] S54: for the vibration of the system, the vibration characteristics are identified and smoothed;

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

[0055] S56: To ensure that the position and posture mapping system of the robot 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, is the position tracking error, Γ is the positive definite adaptive gain matrix, K p and K d is a positive definite gain matrix, λ min (K d ) is the matrix K d The minimum eigenvalue of .

[0060] Furthermore, step S6 specifically includes:

[0061] S61: Considering the real world, the robot arm may touch obstacles during movement;

[0062] S62: construct a three-dimensional virtual obstacle map M of the robot arm and establish a collision detection model;

[0063] S63: For the respective limit values ​​of the motion range and speed of each joint, define the joint constraints of the safe working space of the manipulator, then calculate the minimum distance from the current position of each part of the manipulator 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 designs the obstacle avoidance control law under the speed constraint;

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

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

[0066] The posture acquisition module is responsible for obtaining the teaching operation data of the main robot arm; the angular velocity, position and end posture information of each joint of the main robot arm are collected through the absolute encoder, and the spatial position and posture parameters of the end of the main robot arm are calculated in real time in combination with the forward kinematics calculation;

[0067] The communication control module is used to transmit the data information of the end position of the main robot arm, and adopts model predictive control to solve the motion lag problem caused by communication delay, ensuring the real-time and accuracy of data transmission;

[0068] The posture mapping module is responsible for the posture solution and control of the slave robot arm. It uses proportional scaling and inverse kinematics calculation to solve the posture information of the end of the master robot arm, adjust the joint angle, and combine proportional-integral-differential control to adjust the error between the control output and the actual output in real time to achieve accurate posture mapping between the master and slave robot arms.

[0069] The trajectory prediction module is used to collect and analyze the motion trajectory data of the main robot during the teaching process; the long short-term memory network is used to establish a motion prediction model to predict the posture of the future time step and plan the motion trajectory of the slave robot in advance; at the same time, the attention mechanism is introduced to extract key timing features and continuously optimize the prediction accuracy;

[0070] The motion optimization module is used to filter the position and velocity signals; establish the dynamics model of the robot arm, realize the vibration feature recognition, and design the adaptive control law to compensate the system nonlinearity and external disturbances; ensure the stability of the robot arm motion through the trajectory smoothing algorithm;

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

[0072] The third aspect of the present invention relates to a heterogeneous dual robot arm end posture mapping device, comprising a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the heterogeneous dual robot arm end posture mapping method described above.

[0073] A fourth aspect of the present invention relates to a computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements the above-mentioned heterogeneous dual-manipulator end-point posture mapping method.

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

[0075] 1. The present invention collects the angular velocity, position and end posture signals of the main robot arm joints and calculates the spatial position through the forward kinematics calculation method, and combines model predictive control and proportional-integral-differential control to achieve comprehensive and accurate mapping and prediction of the end posture of the heterogeneous dual robot arms;

[0076] 2. The present invention records the predicted trajectory, real-time posture, motion characteristics and other data, and triggers the corresponding obstacle avoidance mechanism. When the collision detection model judgment value reaches a certain threshold or an abnormal situation occurs, the obstacle avoidance is carried out in time, thereby improving the reliability and safety of the system;

[0077] 3. The present invention makes full use of advanced prediction, control and optimization technologies to significantly improve the performance of the heterogeneous dual-arm robotic system in posture mapping and trajectory planning.

[0078] 4. The present invention solves the technical problem that communication delays in traditional heterogeneous dual-arm mapping systems lead to action lags and uneven motion trajectories, thereby achieving real-time and accuracy of the system; it solves the technical problem that the robot arm motion prediction model in the prior art is insufficiently accurate, making it difficult to accurately predict complex motion trajectories, resulting in unsatisfactory following effects from the robot arm; it solves the technical problem that traditional posture mapping methods lack adaptive capabilities, and when faced with external disturbances and changes in system parameters, the control performance significantly decreases; it solves the technical problem that traditional control methods are difficult to cope with nonlinear interference and workspace constraints, increasing the risk of collision. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 Schematic diagram of the process of a heterogeneous dual-manipulator end-point posture mapping method and system in this embodiment;

[0080] Figure 2 It is a schematic diagram of the posture acquisition module flow of the heterogeneous dual-manipulator end posture mapping of the present invention;

[0081] Figure 3It is a flow chart of the communication control module for the heterogeneous dual-manipulator end-point posture mapping of the present invention;

[0082] Figure 4 It is a flow chart of the posture mapping module of the heterogeneous dual robot arm end posture mapping of the present invention;

[0083] Figure 5 It is a schematic diagram of the trajectory prediction module flow of the heterogeneous dual-manipulator end-point posture mapping of the present invention;

[0084] Figure 6 It is a schematic diagram of the motion optimization module flow of the heterogeneous dual-manipulator end-point posture mapping of the present invention;

[0085] Figure 7 It is a schematic diagram of the safety control module flow of the heterogeneous dual-manipulator end-point posture mapping of the present invention;

[0086] Figure 8 Schematic diagram of the structure of a robot arm prediction system in this embodiment;

[0087] Fig. 9 The structural block diagram of the planning device of the present invention. DETAILED DESCRIPTION

[0088] The specific implementation of the embodiment of the present invention is described in detail below in conjunction with the accompanying drawings. It should be understood that the specific implementation described here is only used to illustrate and explain the embodiment of the present invention, and is not used to limit the embodiment of the present invention.

[0089] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

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

[0091] Example 1

[0092] refer to Figures 1 to 7 A heterogeneous dual-manipulator end-point posture mapping method of the present invention comprises the following steps:

[0093] S1: The instructor manually performs a teaching operation of the main robot arm, and the absolute encoder obtains the angular velocity, position and end posture information of each joint of the main robot arm, collects the original signal and summarizes it, and calculates the spatial position and posture of the end of the main robot arm according to the forward kinematics calculation; the absolute encoder adopts high-precision multi-turn encoding technology, has extremely high resolution and reliability, can maintain the angle value in the case of power failure, and through the established DH parameter model, combined with the real-time motion parameters of each joint, accurately calculates the spatial posture matrix of the end effector of the robot arm relative to the base coordinate system;

[0094] Step S1 specifically includes:

[0095] S11: The instructor manually performs the teaching action on the main robot arm;

[0096] S12: Use an absolute encoder to measure the absolute position information of each main robot arm joint and obtain the rotation angle θ. The angular velocity and angular acceleration of the main robot arm can be obtained through the rotation angle. The angular velocity and angular acceleration measurement formulas are as follows:

[0097]

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

[0099] S13: All collected data are stored in a real-time data acquisition system and sampled at fixed time intervals to form a series of time series data. These data include the angle, angular velocity and acceleration values ​​of each joint;

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

[0101]

[0102] Among them, θ i is the rotation angle of the i-th joint, α i is the torsion angle of the joint (i.e. the angle between adjacent links), a i is the connecting rod length of the ith joint, d i is the offset of the i-th joint, and the transformation matrix T of each joint i Describes the position and posture of the joint in space;

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

[0104] T ee =T 1 ·T 2 ·…·T 6 (11)

[0105] Among them, T 1 ,T 2 ,…,T 6 is the transformation matrix of each joint;

[0106] Homogeneous transformation matrix T eeIt is composed of a 4×4 matrix, which represents the rotation and displacement information of the end effector:

[0107]

[0108] Among them, R ee is the rotation matrix of the end effector, representing its orientation and posture, P ee is the position vector of the end effector representing its spatial coordinates;

[0109] Through the calculation of forward kinematics, we can obtain the spatial position and posture of the end effector;

[0110] The position can be obtained by the transformation matrix T ee The translation part P ee get:

[0111]

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

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

[0114]

[0115] Among them, R 11 ,R 12 ,…,R 33 Represents R ee The value of the corresponding position of the matrix;

[0116] The Euler angle of the posture is obtained by the rotation matrix R ee Get:

[0117]

[0118] Among them, θ x ,θ y ,θ z Represents the Euler angle in the ZYX rotation order;

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

[0120]

[0121] Here, the quaternion is expressed as q=(w,x,y,z).

[0122] S2: In order to achieve accurate prediction, deep learning is adopted to collect the motion trajectory data of the main robot arm during the teaching process, and a motion prediction model is established using the Long Short-Term Memory network to predict the posture of future time steps, plan the motion trajectory of the slave robot arm in advance, and extract key timing features with the help of the attention mechanism to optimize the prediction accuracy; the LSTM network adopts a multi-layer stacking structure, contains 128 hidden layer neurons, and sets a sliding prediction window of 10 time steps. By introducing a multi-head self-attention mechanism, adaptive weighting of features of different time scales is achieved, and the network hyperparameters are optimized by combining the cross-validation method, which significantly improves the generalization ability and prediction accuracy of the model.

[0123] Step S2 specifically includes:

[0124] S21: Based on the regular characteristics of the teaching process of the main robot arm, a deep learning method is used to analyze the teaching data and collect a motion trajectory sequence containing position and posture information;

[0125] S22: Collect the motion trajectory sequence data {x 1 ,x 2 ,...,x T},in Represents the pose vector at time t, which contains position and pose information, T is the sequence length, and d is the pose vector dimension;

[0126] S23: Build an LSTM encoder network, update the memory unit state through the input gate, forget gate and output gate, and extract the 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 =ot ⊙tanh(c t ) (twenty two)

[0132] Among them, i t 、f t , o t They are input gate, forget gate and output gate respectively, c t is the memory unit state, h t is the hidden state output, W i , W f , W o , W c is the weight matrix, b i 、b f 、b o 、b c is the bias vector;

[0133] The attention mechanism is introduced to enhance the model's perception of 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, we get the context vector:

[0139]

[0140] Among them, v a is the attention vector parameter, W a is the attention weight matrix, s {j-1} is the state of the decoder at the previous moment;

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

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

[0143]

[0144] Among them, s t is the decoder status, is the predicted pose output, W p and b pis the prediction layer parameter;

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

[0146]

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

[0148] Among them, y t is the true pose, θ is all the trainable parameters of the model, and λ is the regularization coefficient;

[0149] S25: Use the trained model to predict the main robot arm posture sequence in real time and obtain the posture prediction value of the future prediction step length Where k is the prediction step length;

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

[0151]

[0152] where f plan is the trajectory planning function, τ is the trajectory of the slave robot obtained by planning;

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

[0154]

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

[0156] S3: According to the terminal posture of the main robot arm, the communication module transmits data information and adopts model predictive control to solve the action lag caused by communication delay; the model predictive controller establishes a system dynamic prediction model, online rolling optimizes the control sequence in the future, where the prediction time domain is set to 500ms and the control time domain is 100ms, and uses the quadratic programming algorithm to solve the optimal control quantity, thereby effectively compensating for the system response lag caused by communication delay;

[0157] Step S3 specifically includes:

[0158] S31: converting the signal data related to the main robot arm to be transmitted into a discrete time state space model of the system;

[0159] x(k+1)= A x(k)+ Bu(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, A, B, and C are the state matrix, input matrix, and output matrix of the system respectively;

[0162] S32: Assume that there is a time delay d in the information transfer 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: In order to handle the delay, the state vector of the system can be expanded to include the state vector of the past control input. Define the extended state vector for:

[0164]

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

[0166] The expanded state space model is:

[0167]

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

[0169] The specific form is:

[0170]

[0171]

[0172] S34: Substituting into the MPC optimization problem is usually to minimize the objective function in the prediction time domain N while satisfying the system constraints. The MPC optimization problem considering the delay can be expressed as:

[0173]

[0174] subject to:

[0175]

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

[0177] x min ≤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 output error and control input, respectively, u min and u max are the upper and lower limits of the control input;

[0180] S35: By incorporating the control inputs of the past d time steps into the state vector, MPC can consider the effect of time delay during the optimization process, thereby generating a more optimal control input sequence and completing the signal transmission problem of the master and slave manipulators;

[0181] S4: The slave robot uses proportional scaling and inverse kinematics to solve the posture information of the end of the master robot, adjust the joint angle, and then adjust the error between the control output and the actual output in combination with proportional-integral-derivative control to achieve the mapping of the end posture of the master robot; the PID controller adopts an adaptive parameter adjustment strategy, dynamically adjusts the three gain parameters of proportion, integration and differentiation according to the real-time operating status of the system, solves the inverse kinematics equation through the Jacobian matrix, and uses the gradient descent method to iteratively optimize the joint angle solution to ensure that the slave robot can accurately follow the motion trajectory of the master robot;

[0182] Step S4 specifically includes:

[0183] S41: In view of the differences in size and workspace between the master and slave manipulators, a scaling method is used to achieve posture mapping;

[0184] S42: The end pose of the 6-DOF main manipulator is described by the homogeneous transformation matrix T master express:

[0185]

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

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

[0188]

[0189] in, Indicates the scaling factor, reflecting the proportional relationship between the lengths of the master and slave manipulators;

[0190] S43: Based on the target pose matrix T slave , the inverse kinematics method is used to solve the target angles of each joint of the robot arm in turn:

[0191] First joint angle:

[0192]

[0193] Second and third joint angles:

[0194]

[0195] Fourth, fifth and sixth joint angles:

[0196]

[0197] Control the slave robot to perform corresponding joint angle movements to complete the master-slave mapping;

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

[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 an accurate response according to the real-time motion status of the master robot:

[0200]

[0201] Among them, the proportional term K p It can respond quickly to errors, ensuring that the slave arm can adjust its position immediately. 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 to predict changes in errors and reduce overshoot and oscillation.

[0202] S5: Using an extended Kalman filter to filter the position and velocity signals, establish a mechanical arm dynamics model, realize vibration feature recognition, design an adaptive control law, compensate for system nonlinearity and external disturbances, and use a trajectory smoothing algorithm to ensure the stability of the mechanical arm motion; the extended Kalman filter adopts an adaptive noise covariance matrix design, dynamically adjusts the credibility of the system state estimation by online evaluation of the measurement data quality, and constructs an accurate dynamics model that considers factors such as joint friction, Coriolis force and gravity, and combines the fifth-order polynomial interpolation method to achieve trajectory smoothing, effectively suppressing the vibration and jitter of the mechanical arm during high-speed motion;

[0203] Step S5 specifically includes:

[0204] S51: According to the actual physical size parameters of the robot arm, a dynamic model including inertia matrix, Coriolis force and centrifugal force matrix, and gravity matrix is ​​established:

[0205]

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

[0207] The dynamic model is transformed into a state space equation, the state vector and observation vector are defined, and process noise and measurement noise are considered:

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

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

[0210] The state vector Observation vector y k is the position and velocity measurement, w k and v k are process noise and measurement noise respectively, and satisfy w k ~N(0,Q k ), v k ~N(0,R k );

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

[0212] Status prediction:

[0213]

[0214] Forecast covariance update:

[0215]

[0216] in is the Jacobian matrix of the state equation;

[0217] Perform an extended Kalman filter update, compute the Kalman gains and update the state estimate and covariance matrix:

[0218] Calculate the Kalman gain:

[0219]

[0220] State estimate update:

[0221]

[0222] Covariance matrix update:

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

[0224] in is the Jacobian matrix of the observation equation;

[0225] S53: Design an adaptive control law that includes dynamic parameter estimates, position tracking errors, and a positive definite gain matrix:

[0226]

[0227] in, are the estimated values ​​of the system dynamics parameters, is the position tracking error, K p and K d is a positive definite gain matrix;

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

[0229]

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

[0231] S54: For the vibration of the system, vibration characteristics are identified and smoothed:

[0232] Calculate the system vibration energy:

[0233]

[0234] Define vibration indicators:

[0235]

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

[0237] For the vibration system, cubic spline interpolation is used to generate a smooth trajectory and perform trajectory smoothing:

[0238] q d (t) = a 0 +a 1 t+a 2 t 2 +a 3 t 3 (57)

[0239]

[0240] Among them, the coefficient a 0 、a 1 、a 2 、a 3 Solving for q by using boundary conditions d (t 0 )=q 0 ,q d (t f )=q f ,

[0241] S55: To ensure that the position mapping system of the robot arm is stable, a Lyapunov function is constructed to determine whether it is stable:

[0242]

[0243] Prove that its derivative satisfies:

[0244]

[0245] Among them, λ min (K d ) is the matrix K d The minimum eigenvalue of .

[0246] S6: For workspace constraints and safety control, a virtual obstacle map is established to limit the safe workspace, detect potential collision risks in real time, automatically plan obstacle avoidance trajectories, set position and speed limits, and ensure reliable operation of the system;

[0247] Step S6 specifically includes:

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

[0249] S62: Construct a three-dimensional virtual obstacle map of the robot arm's motion environment and mark the obstacle area and free space in a binary form:

[0250]

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

[0252] S63: Define the angle and angular velocity constraints of each joint of the robot arm to ensure that the robot arm moves within a safe working space:

[0253]

[0254] Among them, q i and Represent the angle and angular velocity of the i-th joint respectively, n is the number of joints in the robot arm, and the range of motion and speed of each joint have their own limit values;

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

[0256]

[0257] Where R(q) represents the spatial point set of the current configuration of the robot, O is the obstacle point set, ∥·∥ 2 represents the Euclidean distance;

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

[0259]

[0260] Among them, d safe is the safety distance threshold, α is the attenuation coefficient, and this function triggers obstacle avoidance when the distance is less than the safety threshold;

[0261] Design the speed-constrained obstacle avoidance control law and generate the obstacle avoidance velocity component through the gradient of the safety index function:

[0262]

[0263] Among them, v d is the expected speed, k v For obstacle avoidance gain, is the gradient of the safety index function, which is used to generate the obstacle avoidance velocity component;

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

[0265]

[0266] Set the position and velocity limits to:

[0267]

[0268] Among them, p base is the position of the robot base, R max is the maximum working radius, v max is the maximum permissible speed.

[0269] Example 2

[0270] refer to Figure 8 , a heterogeneous dual-manipulator end-point posture mapping system of the present invention comprises:

[0271] The posture acquisition module is responsible for obtaining the teaching operation data of the main robot arm; the angular velocity, position and end posture information of each joint of the main robot arm are collected through the absolute encoder, and the spatial position and posture parameters of the end of the main robot arm are calculated in real time in combination with the forward kinematics calculation;

[0272] The communication control module is used to transmit the data information of the end position of the main robot arm, and adopts model predictive control to solve the motion lag problem caused by communication delay, ensuring the real-time and accuracy of data transmission;

[0273] The posture mapping module is responsible for the posture solution and control of the slave robot arm. It uses proportional scaling and inverse kinematics calculation to solve the posture information of the end of the master robot arm, adjust the joint angle, and combine proportional-integral-differential control to adjust the error between the control output and the actual output in real time to achieve accurate posture mapping between the master and slave robot arms.

[0274] The trajectory prediction module is used to collect and analyze the motion trajectory data of the main robot during the teaching process; the long short-term memory network is used to establish a motion prediction model to predict the posture of the future time step and plan the motion trajectory of the slave robot in advance; at the same time, the attention mechanism is introduced to extract key timing features and continuously optimize the prediction accuracy;

[0275] The motion optimization module is used to filter the position and velocity signals; establish the dynamics model of the robot arm, realize the vibration feature recognition, and design the adaptive control law to compensate the system nonlinearity and external disturbances; ensure the stability of the robot arm motion through the trajectory smoothing algorithm;

[0276] The safety control module is responsible for workspace constraints and safety management; establishes a virtual obstacle map and limits the safe workspace, detects potential collision risks in real time, and automatically plans obstacle avoidance trajectories; sets position and speed limit thresholds, monitors the system operation 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-manipulator end posture mapping device, comprising a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the heterogeneous dual-manipulator end posture mapping method described above.

[0279] Example 4

[0280] The present invention provides a computer-readable storage medium having a program stored thereon, and when the program is executed by a processor, the above-mentioned heterogeneous dual-manipulator end-point posture mapping method is implemented.

[0281] The present invention also provides Fig. 9 The one shown corresponds to Figure 1 Schematic diagram of the planning device. Fig. 9 As mentioned above, at the hardware level, the planning device includes a processor, an internal bus, a network interface, a memory, and a non-volatile memory, and may also include other hardware required for the business. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it to achieve the above Figure 1 Of course, in addition to the software implementation, the present invention does not exclude other implementations, such as logic devices or a combination of software and hardware, etc., that is, 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 only an embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A heterogeneous dual-manipulator end-point pose mapping method, characterized in that: The following steps are involved: S1: Manually perform the teaching operation of the main robot arm, obtain the angular velocity, position and end posture information of each joint of the main robot arm, and calculate the spatial position and posture of the end of the main robot arm; S2: Collect the motion trajectory data of the master robot during the teaching process, establish a long short-term memory network model, predict the position and posture of the future time step, and plan the motion trajectory of the slave robot; S3: Data information is transmitted through the communication module, and model predictive control is used to solve the communication delay problem; S4: The slave robot uses scaling and inverse kinematics to solve the posture information of the end of the master robot, adjust the joint angle, and adjust the error in combination with proportional-integral-differential control; S5: Use extended Kalman filter to filter the position and velocity signals, establish the dynamic model of the robot arm, design the adaptive control law, and use the trajectory smoothing algorithm; S6: Create a virtual obstacle map, limit the safe working space, detect potential collision risks in real time, plan obstacle avoidance trajectories, and set position and speed limits.

2. A heterogeneous dual-manipulator end-point pose mapping method according to claim 1, characterized in that: Step S1 specifically includes: S11: The instructor manually performs a teaching action on the main robot arm; S12: using an absolute encoder to measure the absolute position information of each main robot arm joint to obtain a rotation angle θ, the main robot arm angular velocity ω and angular acceleration α can be obtained through the rotation angle θ; S13: All collected data are stored in a real-time data acquisition system and sampled at fixed time intervals to form a series of time series data, which include angles, angular velocities and acceleration values ​​of each joint; S14: The spatial position and posture of the end effector are obtained through forward kinematics calculation.

3. A heterogeneous dual-manipulator end-point pose mapping method according to claim 1, characterized in that: Step S2 specifically includes: S21: Based on the possible regularity of the teaching process of the main robot arm, deep learning is used to analyze its teaching process; S22: Collect the motion trajectory sequence data {x1, x2, ..., x T },in Represents the pose vector at time t, which contains position and pose information, T is the sequence length, and d is the pose vector dimension; S23: Build an LSTM-based encoder network, update the state, and introduce the attention mechanism to calculate the context vector c t , then build the decoder network and combine the context vector to predict the future pose S24: Define the loss function for model training: L total =L pose +L reg (3) Among them, y t is the true pose, θ is all the trainable parameters of the model, and λ is the regularization coefficient; S25: Use the trained model to make real-time predictions on the input main robot arm posture sequence to obtain the posture prediction value of the future time step Where k is the prediction step length; S26: Based on the predicted pose sequence, plan the motion trajectory of the slave robot where f plan is the trajectory planning function, and τ is the trajectory of the slave robot obtained by planning. And the prediction error is monitored in real time When the error exceeds the preset threshold ∈, the model is triggered to be fine-tuned online.

4. A heterogeneous dual-manipulator end-point pose mapping method according to claim 1, characterized in that: Step S3 specifically includes: S31: converting the posture signal data related to the main robot arm to be transmitted into a discrete time state space model of the system; S32: Assume that there is a time delay d in the information transfer 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; S33: To handle the delay, the state vector of the system is expanded to include the state vector of the past control input; S34: Substituting into the MPC optimization problem is usually to minimize the objective function in the prediction time domain N while satisfying the system constraints; the MPC optimization problem considering the delay can be expressed as: subject to: u min ≤u(k+i|k)≤u max x min ≤x(k+i|k)≤x max (4) Where: u(k+i|k) is the i-th control input predicted at time k, y(k+i|k) is the i-th output predicted at time k, is the predicted i-th state at time k, y ref (k+i) is the reference output, Q and R are weight matrices used to penalize output error and control input, respectively, u min and u max are the upper and lower limits of the control input; x min and x max Indicates minimum and maximum limits on state variables; S35: By incorporating the control inputs of the past d time steps into the state vector, MPC can consider the impact of time delay in the optimization process, thereby generating a more optimal control input sequence and solving the signal transmission problem of the master-slave manipulator.

5. A heterogeneous dual-manipulator end-point pose mapping method according to claim 1, characterized in that: Step S4 specifically includes: S41: In view of the differences in size and workspace between the master and slave manipulators, the end position of the master manipulator is mapped to the workspace of the slave manipulator by scaling; S42: By scaling the posture ratio of the end of the main robot arm, the target posture matrix T of the slave robot arm is obtained. slave ; S43: According to 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 achieve the master-slave robot arm mapping; S44: In feedback control, PID control is used to correct the error between the master and slave manipulators; S45: The PID controller adjusts the control input u(t) in real time according to the current error e(t) to ensure that the slave robot can make an accurate response according to the real-time motion status of the master robot: Among them, the proportional term K p It can respond quickly to errors, ensuring that the slave arm can adjust its position immediately. 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 predict changes in errors and reduce overshoot and oscillation.

6. A heterogeneous dual-manipulator end-point pose mapping method according to claim 1, characterized in that: Step S5 specifically includes: S51: Establishing a dynamic model of the robot arm and its system state space equation according to the current robot arm related dimensions; S52: Execute the extended Kalman filter prediction step to perform state prediction and forecast covariance update Perform the extended Kalman filter update step and calculate the Kalman gain State Estimate Update Covariance matrix update P k|k =(IK k H k ) k|k-1 ; The state vector Observation vector y k is the position and velocity measurement, w k and v k are process noise and measurement noise respectively, and satisfy w k ~N(0,Q k ), v k ~N(0,R k ), is the Jacobian matrix of the state equation, is the Jacobian matrix of the observation equation; S53: Design of adaptive control law and adaptive term τ ad The renewal law of S54: for the vibration of the system, the vibration characteristics are identified and smoothed; S55: Calculate the system vibration energy and define the vibration index. When the system vibration energy V i Exceeds the preset threshold V {th} When , the vibration suppression control is triggered; for the vibration system, cubic spline interpolation is used to generate a smooth trajectory and perform trajectory smoothing; S56: To ensure that the position and posture mapping system of the robot arm tends to be stable, a Lyapunov function is constructed to determine whether it is close to stability; Prove that its derivative satisfies: in, is the position tracking error, Γ is the positive definite adaptive gain matrix, K p and K d is a positive definite gain matrix; min (K d ) is the matrix K d The minimum eigenvalue of .

7. A heterogeneous dual-manipulator end-point pose mapping method according to claim 1, characterized in that: Step S6 specifically includes: S61: Considering the real world, the robot arm may touch obstacles during movement; S62: construct a three-dimensional virtual obstacle map M of the robot arm and establish a collision detection model; S63: For the respective limit values ​​of the motion range and speed of each joint, define the joint constraints of the safe working space of the manipulator, then calculate the minimum distance from the current position of each part of the manipulator 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 designs the obstacle avoidance control law under the speed constraint; S64: The system updates the safety status of the robotic arm mapping system in real time according to the set position and speed constraints.

8. A heterogeneous dual-manipulator end-point pose mapping system, characterized in that: include: The posture acquisition module is responsible for acquiring the teaching operation data of the main robot arm; The angular velocity, position and end posture information of each joint of the main robot arm are collected through the absolute encoder, and the spatial position and posture parameters of the end of the main robot arm are calculated in real time by combining the forward kinematics calculation; The communication control module is used to transmit the data information of the end position of the main robot arm, and adopts model predictive control to solve the motion lag problem caused by communication delay, ensuring the real-time and accuracy of data transmission; The posture mapping module is responsible for the posture solution and control of the slave robot arm. It uses proportional scaling and inverse kinematics calculation to solve the posture information of the end of the master robot arm, adjust the joint angle, and combine proportional-integral-differential control to adjust the error between the control output and the actual output in real time to achieve accurate posture mapping between the master and slave robot arms. The trajectory prediction module is used to collect and analyze the motion trajectory data of the main robot during the teaching process; the long short-term memory network is used to establish a motion prediction model to predict the posture of the future time step and plan the motion trajectory of the slave robot in advance; at the same time, the attention mechanism is introduced to extract key timing features and continuously optimize the prediction accuracy; Motion optimization module, used to filter position and velocity signals; Establish a dynamic model of the robot arm, realize vibration feature recognition, and design an adaptive control law to compensate for system nonlinearity and external disturbances; ensure the stability of the robot arm movement through a trajectory smoothing algorithm; The safety control module is responsible for workspace constraints and safety management; establishes a virtual obstacle map and limits the safe workspace, detects potential collision risks in real time, and automatically plans obstacle avoidance trajectories; sets position and speed limit thresholds, monitors the system operation status in real time, and ensures the reliable operation of the dual robotic arm system.

9. A heterogeneous dual-manipulator end-point pose mapping device, characterized in that: It comprises a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, it is used to implement the heterogeneous dual robot arm end pose mapping method described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: A program is stored thereon, and when the program is executed by a processor, the heterogeneous dual-manipulator end pose mapping method described in any one of claims 1 to 7 is implemented.

Images