A somatic cognition-constraint integrated welding robot control method based on IESKF-MCP-RTCP
By employing the IESKF-MCP-RTCP method, combined with visual sensors and adaptive strategies, the posture control problem of welding robotic arms in confined spaces was solved, achieving high-precision, real-time welding control and improving welding quality and safety.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- BEIJING UNIV OF POSTS & TELECOMM
- Filing Date
- 2026-05-19
- Publication Date
- 2026-07-21
Smart Images
Figure CN122425698A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of industrial robot control and artificial intelligence, and mainly relates to a welding robot control method based on IESKF (Iterated Error-State Kalman Filter) - MCP (Mixed-Integer Convex Programming) - RTCP (Rotating Tool Center Point) embodied cognition-constraint integrated control method. Background Technology
[0002] Currently, the existing technology for addressing the posture stability control problem of welding robotic arms in confined spaces mainly utilizes traditional motion control methods. The process involves first performing kinematic modeling of the robotic arm, designing a controller (such as PID or feedforward control), and then correcting errors through encoder feedback. In this traditional control process, the accuracy of state estimation and the ability of path planning to handle complex constraints are key factors affecting the final control performance.
[0003] For state estimation in attitude control, commonly used methods include Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF). EKF has poor applicability, while UKF, although highly accurate in nonlinear systems, involves large computational loads and struggles to guarantee real-time performance. Furthermore, due to the complex environment and severe sensor noise interference within confined spaces, inaccurate state estimation and large trajectory tracking deviations may occur during attitude control, thus affecting welding quality and operational safety.
[0004] For path planning and constraint handling problems, traditional model predictive control (MPC) has the ability to handle multiple constraints, but in practical applications, online solution takes a long time, resulting in insufficient real-time performance. In addition, it has problems such as strong dependence on model accuracy and difficulty in handling mixed integer constraints (such as obstacle avoidance selection). In particular, for welding torches that need to rotate around the tool center point (RTCP), if the nonlinear error caused by rotation is not effectively compensated, it will significantly reduce the accuracy of the end trajectory and affect the weld formation quality.
[0005] The application of robotic arm posture control is relatively mature in many fields. For example, PID control, adaptive control, and various intelligent algorithms have achieved good trajectory tracking and posture adjustment in industrial assembly, painting, and material handling. With the increasing demand for confined space operations in fields such as intelligent manufacturing, shipbuilding, and pressure vessel welding, relatively higher requirements have been placed on control accuracy, real-time performance, and the ability to handle complex constraints. Summary of the Invention
[0006] To achieve real-time and accurate posture stability control of welding robotic arms within confined spaces and meet the requirements of high-precision welding, this invention provides an embodied cognition-constraint integrated welding robotic arm control method based on IESEKF-MCP-RTCP. This method effectively reduces posture errors and response time, improves the posture stability and trajectory tracking accuracy of the robotic arm within confined spaces, and provides real-time and accurate control commands for automated welding in complex environments, enabling the robotic arm to complete welding operations with high quality.
[0007] The specific steps are as follows:
[0008] Step 1, targeting At any given moment, point cloud data of the weld area is collected by a vision sensor at the end of the robotic arm, and the discrete point set of the weld centerline is filtered out. :
[0009] First, collect a point cloud dataset and iterate through it sequentially, selecting the current point cloud dataset. As retrieval points, calculate retrieval points respectively. exist Left and right deviation of direction and in Left-right deviation in direction;
[0010] They are respectively:
[0011]
[0012]
[0013] For all points in the point cloud dataset Minimum coordinates; For all points in the point cloud dataset Maximum coordinate value; For all points in the point cloud dataset Minimum coordinates; For all points in the point cloud dataset Maximum coordinate value;
[0014] Then, the left and right deviations in the two directions are used to determine the search point. Does the following judgment condition meet? If so, then set the point to... Mark it as a candidate center point and add it to the center point set. .
[0015] The judgment criteria are:
[0016] and
[0017] In the formula, , They are respectively direction and Symmetry threshold for orientation setting.
[0018] Finally, by traversing all point clouds, a discrete set of points representing the weld centerline is obtained. .
[0019] Step 2: Calculate the discrete point set of the weld centerline. Fitted curve :
[0020]
[0021] In the formula, This refers to the arc length parameter of the weld curve;
[0022] coefficient matrix , , ;
[0023] Step 3: Mount the welding torch on the robotic arm and calculate the distance between the torch tip and the fitted curve. Position error and attitude error ;
[0024]
[0025] It is the position vector of the welding torch tip in the base coordinate system; This is the point on the weld centerline closest to the current position of the welding torch tip.
[0026]
[0027] In the formula, The attitude error matrix The corresponding rotation angle, The attitude error matrix The corresponding axis of rotation, Here is the attitude error matrix. This represents the transformation operator from an antisymmetric matrix to its corresponding three-dimensional column vector.
[0028] Step 4: Utilizing positional error and attitude error structure The six-dimensional pose error vector at time step :
[0029]
[0030] Step 5: Manually set the posterior state estimate at the initial time. Covariance Matrix The system then calculates the prior state prediction and prior covariance matrix for the next time step, enters the IESKF iteration, updates the welding torch end pose error, and obtains the posterior optimal state estimate. Covariance ;
[0031] The specific steps are as follows:
[0032] Step 501: Manually set the initial settings Posterior state estimation at time 1 and posterior covariance matrix ;
[0033] initial The joint angle portion of the value is assigned the initial encoder reading of the robotic arm, while the non-joint angle portions are all initialized to 0; initial It is a diagonal matrix. .
[0034] Step 502: Using the posterior state estimate and covariance matrix at the initial time step, calculate the prior state prediction and prior covariance matrix at the next time step:
[0035] The formula for predicting the prior state at time t is: ;
[0036] The prior covariance formula matrix is: ;
[0037] In the formula, This is the state transition matrix; for The posterior optimal state estimate at time t. ; for The posterior covariance matrix at time t; Let be the covariance matrix of the Gaussian noise perturbation of the system;
[0038] Step 503, with Prediction of prior states at time 1 and prior covariance matrix As the initial value, it enters the IESKF iteration to update the welding torch end pose error and obtain the posterior optimal state estimate. Covariance ;
[0039] Step 5031, let the iterative index Set the initial iteration state:
[0040] ,
[0041] Step 5032, define the observation function that maps the state to a six-dimensional error vector. For the first In the next iteration, at the current estimate observation function Linearization yields the Jacobian matrix. Kalman gain was then calculated. ;
[0042] Step 5033: Calculate the actual observed values. The residual between the current estimate and the predicted observations ;
[0043]
[0044] Step 5034, utilizing Kalman gain and residual Update the iterative index State estimation Covariance Matrix ,get:
[0045]
[0046]
[0047] In the formula, It is an identity matrix.
[0048] Step 5035, based on the updated state estimate Recalculate Kalman gain And calculate the new residuals. ;
[0049] Step 5036: Determine if the state update requirement is met. or number of iterations If yes, stop the iteration and proceed to step 5038; otherwise, proceed to step 5037.
[0050] Step 5037, return to step 5034, utilize Kalman gain and residual Continue to update the status estimate Covariance Matrix ;
[0051] Step 5038, when finally When the iteration converges, the current time is obtained. The posterior optimal estimate at time:
[0052]
[0053] Step Six: Based on the current situation The six-dimensional pose error vector at time step and optimal state estimation Covariance Constructing an adaptive strategy based on embodied cognition of "environment-action" ;
[0054] First, values are determined based on engineering experience: ;
[0055] for The angle vectors of each joint of the six-axis welding robot arm at any given time; for The angular velocity vectors of each joint of the six-axis welding robot arm at any given time.
[0056] Then, the adaptive policy is calculated.
[0057]
[0058] for Angular velocity vectors of each joint of the six-axis welding robot arm at any given time; This is the discrete time step of the robotic arm, which is the time interval between two adjacent control cycles.
[0059] Step 7: Establish a mathematical model for the mixed-integer optimization problem of MCP:
[0060] The objective function is as follows:
[0061]
[0062] In the formula, The time-domain length predicted by the MCP mixture model is set based on experience; It is the positional deviation between the actual position of the welding torch tip and the corresponding point on the center line of the weld. It is the error between the welding torch tip posture and the desired welding torch tip posture; It represents the change in joint angle between adjacent time points; The L1 norm of a binary variable; , , It is a positive definite weight matrix; This is the penalty coefficient.
[0063] Step 8: Utilize adaptive strategies Solve the MCP mathematical model to obtain the desired welding torch end pose matrix. And extract the desired position vector. and desired pose matrix :
[0064]
[0065] This serves as a reference joint command for the next moment; To calculate the desired pose of the welding torch coordinate system relative to the base coordinate system using a forward kinematics model;
[0066] Step 9: Assess the desired position vector Make corrections, combining them with the desired pose matrix. The joint motion angle command is obtained by solving the problem analytically. :
[0067] First, from the desired pose matrix Solving the inverse kinematics yields the following candidate solutions for the angles of each joint:
[0068]
[0069] In the formula, For the first A set of candidate solutions; The corrected expected position is obtained by superimposing compensation for RTCP errors;
[0070]
[0071] This refers to the positional shift caused by rotation; This is a dynamic speed error compensation term;
[0072] Then, the optimal joint command is selected from all candidate solutions, taking into account the following factors: eliminating joint commands that exceed the allowed joint range. The solution; prioritize the one that is consistent with the previous one. Time-optimal estimation The solution with the smallest difference is used as the pair. The optimal joint angle predicted at each moment; that is: ;
[0073] For solutions that are close together, prioritize those that are collision-free and far from singular configurations.
[0074] Step 10: Find the optimal solution The command is sent to the underlying servo driver for execution, completing trajectory tracking for the current control cycle. The command affects the actual joint angle in the next cycle, thus closing the loop.
[0075] Advantages of this invention:
[0076] (1) This invention improves the end pose estimation accuracy and robustness of the welding gun mounted on the robotic arm by fusing visual and joint encoder information through IESKF, and is especially suitable for scenarios where sensors are limited in a confined space.
[0077] (2) The present invention introduces MCP to handle complex constraints, which can simultaneously consider multiple constraints such as path tracking, obstacle avoidance, and joint limit, and ensure real-time solution efficiency.
[0078] (3) The present invention combines RTCP nonlinear error compensation, which effectively suppresses the trajectory deviation caused by the rotation of the welding torch and significantly improves the welding quality.
[0079] (4) The present invention introduces an embodied intelligent adaptive strategy, which realizes adaptive decision-making and dynamic interaction capabilities in complex environments, thereby improving motion safety and trajectory tracking accuracy.
[0080] (5) The present invention has good versatility and scalability, and can be applied to welding robotic arms of different configurations and various confined space operation environments. Attached Figure Description
[0081] Figure 1 This is a flowchart of the embodied cognition-constraint integrated welding robot arm control method based on IESKF-MCP-RTCP of the present invention;
[0082] Figure 2 This is a detailed schematic diagram of the IESKF iterative update process in this invention;
[0083] Figure 3 This is a flowchart of the adaptive strategy and MCP planning for embodied cognition of "environment-action" in this invention;
[0084] Figure 4 This is a diagram showing the end-point trajectory tracking error of the welding torch mounted on the robotic arm of this invention and other methods. Detailed Implementation
[0085] The invention will be further described in detail below through implementation examples.
[0086] This implementation case uses a six-degree-of-freedom welding robotic arm, equipped with a laser vision sensor and a welding torch at its end, for automated welding of bulkhead welds inside ship cabins; the overall process is as follows: Figure 1 As shown, specifically: First, determine the set of weld center points. Furthermore, the fitting curve of the weld center point set is calculated. Then, the position and attitude errors of the welding torch at the end effector of the robotic arm are calculated, and a six-dimensional pose error vector is constructed. Next, the predicted state and covariance at the current moment are calculated, and the end effector pose error is updated based on IESKF. Furthermore, an embodied cognitive "environment-action" adaptive strategy is constructed. A mathematical model of the mixed integer optimization problem of MCP is established. The reference joint command for the next moment is obtained by solving the MCP constraint model. Solve for the desired end-effector pose matrix. Jacobian matrix at the current joint angle Finally, calculate and compensate for RTCP errors. ; Solving joint motion angle commands through inverse kinematics .
[0087] The IESKF-MCP-RTCP-based embodied cognition-constraint integrated welding robot control method, such as Figure 2 As shown, the specific steps are as follows:
[0088] Step 1, targeting At any given moment, point cloud data of the weld area is collected by a vision sensor at the end of the robotic arm, and the discrete point set of the weld centerline is filtered out. :
[0089] First, collect point cloud datasets. ,in This represents the total number of point clouds; Suppose all points in this dataset The coordinate set is , The set of coordinates is .
[0090] Then, iterate through the sequence and select the current one. As retrieval points, calculate respectively The left and right deviations of the direction are respectively:
[0091]
[0092] and Left and right deviation of direction:
[0093]
[0094] For all points in the point cloud dataset Minimum coordinates; For all points in the point cloud dataset Maximum coordinate value; For all points in the point cloud dataset Minimum coordinates; For all points in the point cloud dataset Maximum coordinate value;
[0095] Next, the left and right deviations in two directions are used to determine the search point. Does the following judgment condition meet? If so, then set the point to... Mark it as a candidate center point and add it to the center point set. :
[0096] If the search point is located within the neighborhood Near the center of the direction, and Approximately equal The same logic applies to the direction; therefore, the judgment condition is: and ;
[0097] In the formula, , They are respectively direction and Symmetry threshold for orientation setting.
[0098] Finally, by traversing all point clouds, a discrete set of points representing the weld centerline is obtained. .
[0099] Step 2: Calculate the discrete point set of the weld centerline. Fitted curve :
[0100] First, define the discrete point set. The Parameters corresponding to each point : ;
[0101] In the formula, Indicates Euclidean distance; .
[0102] Then, a least-squares fit is performed on the weld center curve using a cubic polynomial function, and the weld center curve is expressed as:
[0103] In the formula, The arc length parameter of the weld curve is given; the coefficient matrix is solved by minimizing the error function. :
[0104]
[0105] coefficient matrix , , ;
[0106] Thus, the continuous expression for the weld centerline is obtained:
[0107]
[0108] Step 3: Mount the welding torch on the robotic arm and calculate the distance between the torch tip and the fitted curve. Position error and attitude error ;
[0109] 1) Regarding position error Specifically:
[0110] First, the vector formed by the joint angles of the six-axis welding robot arm is: Adjacent links and The homogeneous transformation matrix between them is:
[0111]
[0112] In the formula, , , , Joints The joint angles, link offsets, link lengths, and link torsion angles are defined. For a welding robot arm, all six joints are rotary joints. The homogeneous transformation matrix of the welding torch coordinate system at the end of the robot arm relative to the base coordinate system is... for: ;
[0113] In the formula, It is the position vector of the welding torch tip in the base coordinate system;
[0114] It is the rotation matrix of the welding torch end coordinate system relative to the base coordinate system.
[0115] Then, samples are taken uniformly across the entire weld curve, and the total number of samples is recorded as follows: Calculate the distance from each sampling point to the current end position, and select the arc length corresponding to the point with the smallest distance as the initial prediction value:
[0116]
[0117] In the formula, It is the arc length corresponding to the point with the smallest distance.
[0118] Finally, the point on the weld centerline closest to the current position of the welding torch tip. for:
[0119]
[0120] Therefore, the position error is .
[0121] 2) Calculate the welding torch at the end of the robotic arm relative to... attitude error :
[0122] First, define the weld tangent vector. After unitizing it, denote it as Define the unit normal vector of the weld as According to the right-hand rule, the unit binormal vector is defined as follows: The welding process requires the welding torch axis to be aligned with the weld seam normal, therefore, the desired welding torch end-position matrix can be constructed as follows:
[0123]
[0124] Therefore, the attitude error matrix is defined as follows:
[0125]
[0126] when When the orientation is perfectly aligned, it indicates that the orientation error matrix is [value missing]. Corresponding rotation angle and rotating axis for:
[0127]
[0128]
[0129] When the value is small, the attitude error vector can be approximated as:
[0130]
[0131] In the formula, This represents a transformation operator from a three-dimensional column vector to its corresponding antisymmetric matrix; This represents the transformation operator from an antisymmetric matrix to its corresponding three-dimensional column vector.
[0132] Step 4: Utilizing positional error and attitude error structure The six-dimensional pose error vector at time step :
[0133]
[0134] Step 5: Manually set the posterior state estimate at the initial time. Covariance Matrix The system then calculates the prior state prediction and prior covariance matrix for the next time step, enters the IESKF iteration, updates the welding torch end pose error, and obtains the posterior optimal state estimate. Covariance ;
[0135] The specific steps are as follows:
[0136] Step 501: Manually set the initial settings Posterior state estimation at time 1 and posterior covariance matrix ;
[0137] initial The joint angle portion of the value is assigned the initial encoder reading of the robotic arm, while the non-joint angle portions are all initialized to 0; initial It is a diagonal matrix. .
[0138] Step 502: Using the posterior state estimate and covariance matrix at the initial time step, calculate the prior state prediction and prior covariance matrix at the next time step:
[0139] Assume the current time is The previous moment The posterior optimal state estimate is And its posterior covariance matrix is ;
[0140] First, based on IESKF, the real state is... Decomposed into nominal states predicted by the system model and the error state to be estimated ,Right now For the robotic arm's state vector: In the formula, This represents the joint angular velocity.
[0141] Assuming at the sampling interval If the angular velocity of the inner joint remains constant, then the state transition equation is: ;
[0142] Where the state transition matrix for: ;
[0143] In the formula, It is a 6×6 identity matrix; This represents the Gaussian noise disturbance in the system.
[0144] Then, based on the optimal estimate from the previous time step, the prior state prediction and prior covariance matrix for the current time step are calculated:
[0145] The formula for predicting the prior state at time t is: ;
[0146] The prior covariance formula matrix is: ;
[0147] In the formula, ; Let be the covariance matrix of the Gaussian noise perturbation of the system;
[0148] Step 503, with Prediction of prior states at time 1 and prior covariance matrix As the initial value, it enters the IESKF iteration to update the welding torch end pose error and obtain the posterior optimal state estimate. Covariance ;
[0149] Step 5031, let the iterative index Set the initial iteration state:
[0150] ,
[0151] Step 5032, define the observation function that maps the state to a six-dimensional error vector. : For the first In the next iteration, at the current estimate observation function Linearization yields the Jacobian matrix. Kalman gain was then calculated. ;
[0152]
[0153]
[0154] In the formula, Let be the covariance matrix of the observation noise.
[0155] Step 5033: Calculate the actual observed values. The residual between the current estimate and the predicted observations ;
[0156]
[0157] Step 5034, utilizing Kalman gain and residual Update the iterative index State estimation Covariance Matrix ,get:
[0158]
[0159]
[0160] In the formula, It is an identity matrix.
[0161] Step 5035, based on the updated state estimate Recalculate Kalman gain And calculate the new residuals. ;
[0162] Step 5036: Determine if the state update requirement is met. or number of iterations If yes, stop the iteration and proceed to step 5038; otherwise, proceed to step 5037.
[0163] The preset convergence threshold,
[0164] Step 5037, return to step 5034, utilize Kalman gain and residual Continue to update the status estimate Covariance Matrix ;
[0165] Step 5038, when finally When the iteration converges, the current time is obtained. The posterior optimal estimate at time:
[0166]
[0167] Step Six: Based on the current situation The six-dimensional pose error vector at time step and optimal state estimation Covariance Constructing an adaptive strategy based on embodied cognition of "environment-action" ;
[0168] In optimal state estimation Based on this, an embodied intelligence modeling method is introduced to unify the robotic arm, environment, and task into a closed-loop system.
[0169] like Figure 3 As shown, firstly, the embodied state vector is defined as: ;
[0170] The environmental constraint description vector is defined as follows: ;
[0171] In the formula, It refers to the number of obstacles; Represents the point set of the robotic arm body To the A set of obstacle points Minimum distance:
[0172]
[0173] Then, establish a "state-action" coupled dynamic model:
[0174]
[0175] In the formula, To control the input, For the first Angular velocity vectors of each joint of the six-axis welding robot arm at any given time; The system disturbance is represented by the model. First-order linearization of the model yields: In the formula, , This is the linearized system matrix.
[0176] Subsequently, an embodied intelligence optimization objective function is constructed. :
[0177]
[0178] In the formula, This is the pose error weight matrix; To control the input weight matrix; Let the environmental interaction cost function be defined as: ;
[0179] Next, design the embodied policy mapping function:
[0180]
[0181] In the formula, , This is the gain coefficient; This represents the repulsion gradient of the obstacle to the joint space.
[0182] Finally, the embodied strategy is used as the initial guide for MCP optimization to generate an initial guess of the joint angle at the next time step:
[0183]
[0184] for Angular velocity vectors of each joint of the six-axis welding robot arm at any given time; This is the discrete time step of the robotic arm, which is the time interval between two adjacent control cycles.
[0185] Step 7: Establish a mathematical model for the mixed-integer optimization problem of MCP:
[0186] Define the prediction time domain length as Decision variables include expectations for the future. The predicted joint angle sequence and auxiliary binary variables at each time step:
[0187]
[0188] In the formula, For at any time The predicted first The joint angle vector at any given time; ( (The number of binary variables) is a binary variable vector used to represent discrete decisions such as obstacle avoidance selection and path branching.
[0189] The initial joint angle guidance amount generated by the embodied cognitive strategy As the initial guess for the joint angle at the first moment in the optimization problem, let For subsequent moments A constant angular velocity model is adopted. Generate initial trajectories to form a complete sequence of initial solutions. This serves as the hot start point for the MCP solver.
[0190] The objective function is as follows:
[0191]
[0192] In the formula, The time-domain length predicted by the MCP mixture model is set based on experience; It is the positional deviation between the actual position of the welding torch tip and the corresponding point on the center line of the weld. It is the error between the welding torch tip posture and the desired welding torch tip posture, and its calculation method is the same as... same; It represents the change in joint angle between adjacent time points; The L1 norm of a binary variable; , , It is a positive definite weight matrix; This is the penalty coefficient.
[0193] The optimization problem must satisfy the following constraints. :
[0194]
[0195] In the formula, Link The location of the key points; , Describe obstacles The A convex sub-region; It is a large constant; A binary variable indicating the linkage. Is it located in this sub-region? is the axis vector of the welding torch; This is the normal vector of the weld.
[0196] In addition, weld matching is achieved by introducing an arc length parameter. Linearization:
[0197]
[0198] Finally, after processing, the MCP constraint model can be constructed as follows:
[0199]
[0200] Step 8: Utilize adaptive strategies Solving the MCP mathematical model yields Simultaneously solve the desired welding torch end pose matrix. Jacobian matrix at the current joint angle And extract the desired position vector. and desired pose matrix :
[0201] First, solve the integer programming model to obtain the optimal solution:
[0202]
[0203] Will As the initial value for solving The optimal solution is contained in time. Predicted future Joint angle sequence at each moment And the optimal value of binary variables The first step in obtaining the sequence Reference joint commands for the next moment: ;
[0204] Then, solve for the desired welding torch end pose matrix. Jacobian matrix at the current joint angle :
[0205] First, based on reference joint commands Using a forward kinematics model, the desired pose of the welding torch coordinate system at the end of the robotic arm relative to the base coordinate system is calculated. :
[0206]
[0207] The matrix is denoted as:
[0208]
[0209] Then, extract the desired position vector from it. And the desired pose matrix:
[0210] .
[0211] Finally, at the joint angle Calculate the Jacobian matrix at this point. For rotational joints Its corresponding Jacobian column vector is:
[0212]
[0213] In the formula, For joints The direction of the rotation axis in the base coordinate system; For the first The position coordinates of the origin of the link coordinate system in the base coordinate system, and .
[0214] Calculations column by column yielded:
[0215]
[0216] Step 9: Assess the desired position vector Make corrections, combining them with the desired pose matrix. The joint motion angle command is obtained by solving the problem analytically. :
[0217] First, from the desired pose matrix By solving the inverse kinematics analytically, the candidate solutions for the angles of each joint are obtained as follows:
[0218]
[0219] In the formula, For the first A set of candidate solutions;
[0220] Calculation and Compensation of RTCP Errors Specifically:
[0221] First, define the offset vector between the tool coordinate system and the tool center point (TCP).
[0222] Let the tool coordinate system of the welding gun be... Its origin is located at the end of the welding wire; the offset vector of the TCP point in the tool coordinate system is: ; This represents the offset along the length of the welding torch (z-axis of the tool coordinate system); For horizontal and vertical offsets.
[0223] The optimal state at the current moment is estimated as follows Corresponding joint angle and angular velocity The current actual pose can be obtained from the positive kinematics. From which can be extracted The actual terminal velocity is From the desired pose It can be extracted from Expected terminal velocity Based on the desired weld trajectory The derivative at that point is calculated.
[0224] Then, calculate the position offset caused by the rotation:
[0225]
[0226] And calculate the dynamic speed error compensation term:
[0227]
[0228] The speed error compensation gain matrix is a positive definite diagonal matrix tuned according to the system bandwidth and response speed requirements.
[0229] Finally, the total compensation is obtained by summing the results, and the desired position is corrected accordingly.
[0230]
[0231] Maintain the expected posture The output remains unchanged, and the corrected desired pose is used for subsequent inverse kinematics solutions.
[0232] Then, the optimal joint command is selected from all candidate solutions, taking into account the following factors: eliminating joint commands that exceed the allowed joint range. The solution; prioritize the one that is consistent with the previous one. Time-optimal estimation The solution with the smallest difference is used as the pair. The optimal joint angle predicted at each moment; that is: ;
[0233] For solutions that are close together, prioritize those that are collision-free and far from singular configurations.
[0234] Step 10: Find the optimal solution The command is sent to the underlying servo driver for execution, completing trajectory tracking for the current control cycle. The command affects the actual joint angle in the next cycle, thus closing the loop.
[0235] Example:
[0236] Step 1: Collect point cloud data of the weld area using a vision sensor installed at the end of the robotic arm.
[0237] Set a symmetry threshold .
[0238] By traversing all point clouds and selecting points that satisfy the symmetry condition, a discrete set of points for the weld centerline is obtained:
[0239] .
[0240] Step 2: Calculate the fitting curve of the weld center point set. :
[0241] The weld center curve is fitted with a cubic polynomial function using least-squares fitting to obtain a continuous expression for the weld center curve:
[0242]
[0243] Among them, the arc length parameter The range is .
[0244] Step 3: Calculate the welding torch at the end of the robotic arm relative to... Position error and attitude error ;
[0245] The current joint angles of the six-axis welding robotic arm are as follows: The terminal position is obtained through forward kinematics. .
[0246] 50 points were sampled evenly on the weld curve, and the arc length corresponding to the nearest point was calculated as follows: The corresponding point coordinates are Therefore, the position error vector is:
[0247]
[0248] The weld tangent vector is The weld normal vector is The binormal vector is The desired attitude matrix is constructed and then compared with the actual attitude matrix to obtain the attitude error angle. The axis of rotation is The attitude error vector is approximately:
[0249]
[0250] Step 4: Construct a six-dimensional pose error vector:
[0251]
[0252] Step 5: Calculate the prior predicted state at the current time. and prior covariance :
[0253] Estimation of the optimal state at the previous moment for ,
[0254] angular velocity is The sampling interval is... The predicted prior state at the current time is calculated as follows: ;
[0255] covariance matrix Update according to the formula.
[0256] Step 6, End-effector pose error update based on IESKF:
[0257] Let the observation noise covariance matrix be... for:
[0258]
[0259] Convergence threshold Maximum number of iterations After three iterations, the state update amount is 0.0008 < 0.001, indicating convergence, and the posterior optimal state estimate is obtained.
[0260]
[0261] Step 7: Construct an adaptive strategy for embodied cognition of "environment-action". :
[0262] There are three obstacles in the current environment, with minimum distances of 15 mm, 22 mm, and 18 mm, respectively. After constructing the embodied state vector, an optimization objective function is designed to generate initial joint angle guidance values:
[0263]
[0264] Step 8: Establish a mathematical model for the mixed-integer optimization problem of MCP:
[0265] Set the prediction time domain binary variables The number of parameters is 5. Construct the objective function and constraints, and initialize the optimization sequence.
[0266] Step nine, solve the MCP constraint model to obtain :
[0267] The branch and bound method is used to solve the problem and obtain the optimal joint angle sequence. The first step is as follows:
[0268]
[0269] Step 10: Solve for the desired end pose matrix. Jacobian matrix at the current joint angle :
[0270] Based on reference joint commands, the desired distal end position obtained from forward kinematics is: The attitude matrix is:
[0271]
[0272] Step 11: Calculate and compensate for RTCP errors :
[0273] Let the TCP offset vector be The current terminal velocity is The expected speed is The calculated rotational offset and velocity error compensation terms, and the final corrected desired position, are as follows: ;
[0274] Step 12: Solve for joint motion angle commands using inverse kinematics :
[0275] Eight candidate solutions were obtained through analytical methods. After eliminating solutions that exceeded the joint range, the solution closest to the optimal estimate at the previous time step was selected.
[0276]
[0277] The instruction was sent to the underlying servo driver, completing the trajectory tracking for the current control cycle.
[0278] To verify the accuracy of this invention in trajectory tracking within a confined space, the end-effector control effects of the method described in this invention and other methods were compared. The resulting curves are shown below. Figure 4 As shown. By Figure 4As can be seen, the tracking error using this method (Group 5) is only 0.05 cm. This indicates that the method of the present invention can effectively suppress the nonlinear error caused by the rotation of the welding torch, significantly improve the trajectory tracking accuracy, and meet the requirements of high-precision welding. The experimental results verify the effectiveness and practicality of the method of the present invention.
Claims
1. A control method for an embodied cognition-constraint integrated welding robotic arm based on IESEKF-MCP-RTCP, characterized in that, The specific steps are as follows: Step 1, targeting At any given moment, point cloud data of the weld area is collected by a vision sensor at the end of the robotic arm, and the discrete point set of the weld centerline is filtered out. And calculate the discrete point set Fitted curve : ； In the formula, This refers to the arc length parameter of the weld curve; coefficient matrix , , ; Step 2: Mount the welding torch on the robotic arm and calculate the distance between the torch tip and the fitted curve. Position error and attitude error and further construct The six-dimensional pose error vector at time step : ； Step 3: Manually set the posterior state estimate at the initial time. Covariance Matrix The system then calculates the prior state prediction and prior covariance matrix for the next time step, enters the IESKF iteration, updates the welding torch end pose error, and obtains the posterior optimal state estimate. Covariance ; Step 4: Based on the current situation The six-dimensional pose error vector at time step and optimal state estimation Covariance Constructing an adaptive strategy based on embodied cognition of "environment-action" ; ； for Angular velocity vectors of each joint of the six-axis welding robot arm at any given time; This is the discrete time step of the robotic arm, which is the time interval between two adjacent control cycles; for The angle vectors of each joint of the six-axis welding robot arm at any given time are determined based on engineering experience: ; for Angular velocity vectors of each joint of the six-axis welding robot arm at any given time; Step 5: Establish a mathematical model for the mixed-integer optimization problem of MCP: The objective function is as follows: ； In the formula, The time-domain length predicted by the MCP mixture model is set based on experience; It is the positional deviation between the actual position of the welding torch tip and the corresponding point on the center line of the weld. It is the error between the welding torch tip posture and the desired welding torch tip posture; It represents the change in joint angle between adjacent time points; The L1 norm of a binary variable; , , It is a positive definite weight matrix; This is the penalty coefficient; Step Six: Utilize Adaptive Strategies Solve the MCP mathematical model to obtain the desired welding torch end pose matrix. And extract the desired position vector. and desired pose matrix : ； This serves as a reference joint command for the next moment; To calculate the desired pose of the welding torch coordinate system relative to the base coordinate system using a forward kinematics model; Step 7: Assess the desired position vector Make corrections, combining them with the desired pose matrix. The joint motion angle command is obtained by solving the problem analytically. The data is then sent to the underlying servo driver for execution, completing the trajectory tracking for the current control cycle.
2. The method as described in claim 1, characterized in that, In step one, First, collect a point cloud dataset and iterate through it sequentially, selecting the current point cloud dataset. As retrieval points, calculate retrieval points respectively. exist Left and right deviation of direction and in Left-right deviation in direction; They are respectively: ； ； For all points in the point cloud dataset Minimum coordinates; For all points in the point cloud dataset Maximum coordinate value; For all points in the point cloud dataset Minimum coordinates; For all points in the point cloud dataset Maximum coordinate value; Then, the left and right deviations in the two directions are used to determine the search point. Does the following judgment condition meet? If so, then set the point to... Mark it as a candidate center point and add it to the center point set. ; The judgment criteria are: and ; In the formula, , They are respectively direction and The symmetry threshold for the direction setting; Finally, by traversing all point clouds, a discrete set of points representing the weld centerline is obtained. .
3. The method as described in claim 1, characterized in that, In step two, the position error is calculated as follows: ； It is the position vector of the welding torch tip in the base coordinate system; This is the point on the weld centerline closest to the current position of the welding torch tip. Attitude error is calculated as follows: ； In the formula, The attitude error matrix The corresponding rotation angle, The attitude error matrix The corresponding axis of rotation, Here is the attitude error matrix. This represents the transformation operator from an antisymmetric matrix to its corresponding three-dimensional column vector.
4. The method as described in claim 1, characterized in that, Step three specifically involves: Step 501: Manually set the initial settings Posterior state estimation at time 1 and posterior covariance matrix ; initial The joint angle portion of the value is assigned the initial encoder reading of the robotic arm, while the non-joint angle portions are all initialized to 0; initial It is a diagonal matrix. ; Step 502: Using the posterior state estimate and covariance matrix at the initial time step, calculate the prior state prediction and prior covariance matrix at the next time step: The formula for predicting the prior state at time t is: ; The prior covariance formula matrix is: ; In the formula, This is the state transition matrix; for The posterior optimal state estimate at time t. ; for The posterior covariance matrix at time t; Let be the covariance matrix of the Gaussian noise perturbation of the system; Step 503, with Prediction of prior states at time 1 and prior covariance matrix As the initial value, it enters the IESKF iteration to update the welding torch end pose error and obtain the posterior optimal state estimate. Covariance ; Step 5031, let the iterative index Set the initial iteration state: ， ； Step 5032, define the observation function that maps the state to a six-dimensional error vector. For the first In the next iteration, at the current estimate observation function Linearization yields the Jacobian matrix. Kalman gain was then calculated. ; Step 5033: Calculate the actual observed values. The residual between the current estimate and the predicted observations ; ； Step 5034, utilizing Kalman gain and residual Update the iterative index State estimation Covariance Matrix ,get: ； ； In the formula, It is the identity matrix; Step 5035, based on the updated state estimate Recalculate Kalman gain And calculate the new residuals. ; Step 5036: Determine if the state update quantity is satisfied. or number of iterations If so, stop the iteration and proceed to step 5038; Otherwise, proceed to step 5037; Step 5037, return to step 5034, utilize Kalman gain and residual Continue to update the status estimate Covariance Matrix ; Step 5038, when finally When the iteration converges, the current time is obtained. The posterior optimal estimate at time: 。 5. The method as described in claim 1, characterized in that, In step seven, First, from the desired pose matrix Solving the inverse kinematics yields the following candidate solutions for the angles of each joint: ； In the formula, For the first Group of candidate solutions; The corrected expected position is obtained by superimposing compensation for RTCP errors; ； This refers to the positional shift caused by rotation; This is a dynamic speed error compensation term; Then, the optimal joint command is selected from all candidate solutions, taking into account the following factors: eliminating joint commands that exceed the allowed joint range. The solution; prioritize the one that is consistent with the previous one. Time-optimal estimation The solution with the smallest difference is used as the pair. The optimal joint angle predicted at each moment; that is: ; For solutions that are close together, prioritize those that are collision-free and far from singular configurations.