Robot trajectory correction method based on model predictive control
By using a model predictive control method, a predictive controller is constructed using the first-layer processing data to compensate for robot trajectory errors in real time, thus solving the problem of insufficient trajectory accuracy during robot wire placement and achieving high-precision and stable wire placement control.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- QUANZHOU HUAZHONG UNIV OF SCI & TECH INST OF MFG
- Filing Date
- 2026-05-12
- Publication Date
- 2026-06-26
AI Technical Summary
Existing technologies struggle to effectively compensate for dynamic disturbances and improve trajectory accuracy during robotic filament laying, especially in complex paths and high-precision control, where systematic deviations and insufficient control precision exist.
A model-based predictive control method is adopted, which constructs a predictive controller using the first-layer processing data, optimizes the parameters using the least squares method, and combines force sensor and encoder data to compensate for robot trajectory errors in real time, thereby achieving high-precision trajectory tracking.
It significantly improves the trajectory accuracy and robustness during the wire laying process, achieving high-precision and stable control without requiring underlying modifications to the robot servo system.
Smart Images
Figure CN122284499A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of filament placement control, and in particular to a robot filament placement trajectory correction method based on model predictive control. Background Technology
[0002] Robotic fiber placement refers to the process of laying composite materials using robots or automated equipment. Traditional fiber placement operations typically require manual positioning, measurement, cutting, and placement, consuming significant manpower and time, and making it difficult to guarantee placement accuracy and efficiency. To improve accuracy and efficiency, automated fiber placement technology using robots has emerged. However, in automated fiber placement operations, due to factors such as high path complexity, workpiece deformation, and uncertain contact stiffness, the robot's execution trajectory often deviates systematically from the ideal trajectory. These trajectory errors and dynamic disturbances usually require correction through traditional dynamic modeling and PID control.
[0003] However, these methods have the following shortcomings: ① The dynamic model is difficult to fully capture all system errors and external disturbances; ② The control accuracy cannot meet the requirements of high complexity and high precision in wire laying tasks; ③ Existing control strategies lack effective compensation for dynamic disturbances (such as contact force fluctuations, uneven workpiece surfaces, etc.).
[0004] Therefore, how to improve the accuracy and robustness of robot control systems through data-driven methods, especially in complex path tracking and force control, is a significant challenge in current technology. Summary of the Invention
[0005] The main objective of this invention is to propose a robot filament placement trajectory correction method based on model predictive control, which can achieve high-precision filament placement control.
[0006] This invention is achieved through the following technical solution:
[0007] The robot filament placement trajectory correction method based on model predictive control includes the following steps:
[0008] Step S1: Use CAM software to plan the filament placement reference trajectory. The robot lays the first layer of filament according to the reference trajectory and collects the first layer processing data during the first layer laying process. The first layer processing data includes the reference pose and velocity of the robot end effector, the actual pose and velocity, and external disturbance data.
[0009] Step S2: Construct a predictive controller to compensate for trajectory errors during the wire laying process based on the first layer processing data;
[0010] Step S3: Use the first-layer processing data to obtain the trajectory error to construct the trajectory error model, and optimize the parameters of the predictive controller by the least squares method. The parameters of the predictive controller include the disturbance term.
[0011] Step S4: Perform an optimization calculation of the predictive controller parameters once in each control cycle, and apply the output control quantity of the predictive controller to the robot joint actuator to compensate for the filament laying trajectory error.
[0012] Furthermore, in step S1, the reference trajectory includes discrete trajectory points in the Cartesian coordinate system, the robot pose corresponding to each discrete trajectory point, and the robot velocity and acceleration.
[0013] Furthermore, in step S1, the external disturbance data includes the contact force of the filament-laying head collected by a force sensor installed at the end of the robot.
[0014] Furthermore, in step S2, the predictive controller is represented as ,in, Let N be the first control variable in the future control output of the controller at time k, and let N be the prediction step size. For predictive controllers, a state-space based discrete model is used. For the robot's predicted pose and velocity at time i, This represents the robot's predicted pose and velocity at time i-1. To predict the control correction amount of the controller output at time i, To predict the future control quantity of the controller output at time i-1, Let be the disturbance at time i-1. Let represent the reference pose and velocity at time i, Q and R be the trajectory tracking error weighting matrix and the total energy consumption weighting matrix, respectively, A be the matrix describing the trajectory error state propagation law, and B be the matrix describing the ability of trajectory correction to suppress trajectory error.
[0015] Furthermore, in step S3, for the disturbance term, the first objective function is minimized. Optimize matrix A, matrix B, and perturbation term. ,in, This is the trajectory error model. Let k be the robot's actual pose and velocity at time k. For the robot's predicted pose and velocity at time k, the perturbation term... This corresponds to external disturbance data.
[0016] Furthermore, in step S3, for the trajectory error model, the trajectory error is first calculated based on the robot's execution trajectory and the reference trajectory, and then the trajectory error is fitted by spline interpolation based on the first layer processing data to form a trajectory error model that is represented as a continuous disturbance curve.
[0017] Furthermore, in step S3, the optimization matrix A, matrix B, and perturbation term are optimized. Then, by minimizing the second objective function Optimize the weight matrices Q and R, where x ref,k+i Let k+i be the reference pose and velocity. , For the weighted average sum, This indicates that the pose and velocity at time k+i are predicted based on information from time k. This represents the control input for predicting time k+i based on information from time k.
[0018] Furthermore, in step S4, the OSQP solver deployed in the control module of the wire laying CNC system is used to perform optimization calculations for the predictive controller.
[0019] As can be seen from the above description of the present invention, compared with the prior art, the present invention has the following beneficial effects:
[0020] The composite material layup process follows the principle of multi-layer repetitive layup. Therefore, this invention proposes to model and correct the robot system using the execution data of the first layer. Specifically, CAM software is first used to plan a reference trajectory for fiber placement. The robot then performs the first layer of fiber placement according to the reference trajectory, collecting the processing data of the first layer. Based on this data, a predictive controller is constructed to compensate for trajectory errors during the fiber placement process. Next, regression analysis is used to obtain the trajectory error from the first layer processing data to construct a trajectory error model. The parameters of the predictive controller are optimized using the least squares method, and the output control quantity of the optimized predictive controller is applied to the robot joint actuators to compensate for the fiber placement trajectory error. Using this invention, no underlying modification to the robot servo system is required; high-precision trajectory tracking and stable control are achieved on the host computer side. This effectively reduces the impact of contact process disturbances and structural flexibility on fiber placement quality, significantly improving trajectory accuracy, consistency, and robustness during the fiber placement process. It has good engineering feasibility and application value. Attached Figure Description
[0021] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.
[0022] Figure 1 This is a flowchart of the present invention.
[0023] Figure 2 This is a schematic diagram of the robot's filament laying operation state according to the present invention. Detailed Implementation
[0024] The present invention will be further described below through specific embodiments.
[0025] like Figure 1As shown, the robot filament placement trajectory correction method based on model predictive control includes the following steps:
[0026] Step S1: Use CAM software to plan the filament placement reference trajectory. The robot lays the first layer of filament according to the reference trajectory and collects the first layer processing data during the first layer laying process. The first layer processing data includes the reference pose and velocity of the robot end effector, the actual pose and velocity, and external disturbance data.
[0027] The process of planning a reference trajectory using CAM software is existing technology. The reference trajectory includes discrete trajectory points in a Cartesian coordinate system, the robot pose corresponding to each discrete trajectory point, and the robot's velocity and acceleration. External disturbance data includes the contact force of the filament-laying head, collected by a force sensor mounted on the robot's end effector. The robot's end effector is also equipped with a laser tracker for acquiring its spatial position and attitude; the robot's movement speed can be obtained through an encoder. Figure 2 The diagram shows the working state of the robot's filament placement. A six-joint robot is used, with the filament placement head located at the robot's end effector.
[0028] Step S2: Construct a predictive controller to compensate for trajectory errors during the wire laying process based on the first layer processing data;
[0029] Specifically, the state-space-based discrete model is represented as follows: , For the robot's predicted pose and velocity at time i, This represents the robot's predicted pose and velocity at time i-1. To predict the control correction amount of the controller output at time i, To predict the future control quantity of the controller output at time i-1, the control quantity is typically a torque, speed, or position command. Let i be the disturbance at time i-1, such as changes in contact force, path deviation, load change, etc. Let A represent the reference pose and velocity at time i. A describes the intrinsic evolution characteristics of the trajectory error state of the filament-laying robot in the discrete-time domain, reflecting the propagation law of the error state when no compensation control input is applied. B describes the degree of influence of the host computer's compensation control input on the trajectory error state, used to characterize the ability of the trajectory correction amount to suppress error dynamics. The trajectory error state is the deviation state of the actual trajectory relative to the reference trajectory, containing two components: end-path pose error and end-velocity error.
[0030] Therefore, the predictive controller is represented as ,in, Let N be the prediction step size, and let Q and R be the trajectory tracking error weighting matrix and total energy consumption weighting matrix, respectively. Both are symmetric positive definite matrices. To find the norm, which is a measure of the size of a vector, specifically, This is a weighted sum of squares. When i=k, This refers to the actual state variables of the system at time k, such as the robot's position, velocity, and attitude. These variables originate from sensor measurements, encoders, vision systems, etc., and are typically real-time state feedback from the robot's end effector. The value x after time k... i Then it is the predicted state variable.
[0031] Step S3: Use the first-layer processing data to obtain the trajectory error to construct the trajectory error model, and optimize the parameters of the predictive controller by the least squares method. The parameters of the predictive controller include the disturbance term.
[0032] The goal of the trajectory error model is to fit the error distribution function between the robot's executed trajectory and the ideal trajectory; that is, to first calculate the trajectory error based on the robot's executed trajectory and the reference trajectory. Based on the first-layer processing data, the trajectory error is fitted using spline interpolation to form a trajectory error model that represents a continuous perturbation curve, where t is the time parameter, e(t) is the trajectory error, and e real (𝑡) represents the robot's execution trajectory, e ref (x) represents the reference trajectory. The specific process of spline interpolation is existing technology. The path error model is used to provide initial parameters for the predictive controller, such as: disturbance terms in the state equation, system constraints, and initial error prediction values.
[0033] To optimize the performance of the predictive controller, it is necessary to minimize the system's prediction error and compensate for disturbance terms. For the disturbance terms, this is achieved by minimizing the first objective function. Optimize matrix A, matrix B, and perturbation term. This yields the optimal perturbation model parameters, where... For trajectory error model, Let k be the robot's actual pose and velocity at time k. For the robot's predicted pose and velocity at time k, the perturbation term... This corresponds to external disturbance data.
[0034] Optimize matrix A, matrix B, and perturbation term Then, it is also necessary to minimize the second objective function. Optimize the weight matrices Q and R, where x ref,k+i Let k+i be the reference pose and velocity. , For the weighted average sum, This indicates that the pose and velocity at time k+i are predicted based on information from time k. This represents the control input for predicting time k+i based on information from time k.
[0035] Step S4: Perform an optimization calculation of the predictive controller parameters once in each control cycle, and apply the output control quantity of the predictive controller to the current of the robot joint actuator to compensate for the filament laying trajectory error.
[0036] Specifically, the OSQP solver deployed in the control module of the wire-laying CNC system is used to perform optimization calculations for the predictive controller. An optimization calculation is completed within each control cycle (30~50ms).
[0037] In this invention, the terms "first," "second," and "third," etc., are used only to distinguish similar objects and are not necessarily used to describe a specific order or sequence, nor should they be construed as indicating or implying relative importance. The use of terms such as "upper," "lower," "left," "right," "front," and "rear" to indicate orientation or positional relationships is based on the orientation or positional relationships shown in the accompanying drawings and is only for the convenience of describing the invention, not to indicate or imply that the device referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, it should not be construed as a limitation on the scope of protection of this invention. Those skilled in the art can understand the specific meaning of the above terms in this application according to the specific circumstances.
[0038] Furthermore, in the description of this application, unless otherwise stated, "multiple" means two or more. "And / or" describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. The character " / " generally indicates that the preceding and following related objects have an "or" relationship.
[0039] The above are merely specific embodiments of the present invention, but the design concept of the present invention is not limited thereto. Any non-substantial modifications made to the present invention using this concept shall be considered as infringing upon the protection scope of the present invention.
Claims
1. A robot filament placement trajectory correction method based on model predictive control, characterized in that: Includes the following steps: Step S1: Use CAM software to plan the filament placement reference trajectory. The robot lays the first layer of filament according to the reference trajectory and collects the first layer processing data during the first layer laying process. The first layer processing data includes the reference pose and velocity of the robot end effector, the actual pose and velocity, and external disturbance data. Step S2: Construct a predictive controller to compensate for trajectory errors during the wire laying process based on the first layer processing data; Step S3: Use the first-layer processing data to obtain the trajectory error to construct the trajectory error model, and optimize the parameters of the predictive controller by the least squares method. The parameters of the predictive controller include the disturbance term. Step S4: Perform an optimization calculation of the predictive controller parameters once in each control cycle, and apply the output control quantity of the predictive controller to the robot joint actuator to compensate for the filament laying trajectory error.
2. The robot filament placement trajectory correction method based on model predictive control according to claim 1, characterized in that: In step S1, the reference trajectory includes discrete trajectory points in the Cartesian coordinate system, the robot pose, robot velocity, and acceleration corresponding to each discrete trajectory point.
3. The robot filament placement trajectory correction method based on model predictive control according to claim 2, characterized in that: In step S1, the external disturbance data includes the contact force of the filament-laying head collected by a force sensor installed at the end of the robot.
4. The robot filament placement trajectory correction method based on model predictive control according to claim 1, 2, or 3, characterized in that: In step S2, the predictive controller is represented as ,in, Let N be the first control variable in the future control output of the controller at time k, and let N be the prediction step size. For predictive controllers, a state-space based discrete model is used. For the robot's predicted pose and velocity at time i, This represents the robot's predicted pose and velocity at time i-1. To predict the control correction amount of the controller output at time i, To predict the control correction amount of the controller output at time i-1, Let be the disturbance at time i-1. Let represent the reference pose and velocity at time i, Q and R be the trajectory tracking error weighting matrix and the total energy consumption weighting matrix, respectively, A be the matrix describing the propagation law of trajectory error state, and B be the matrix describing the ability of trajectory correction to suppress trajectory error.
5. The robot filament placement trajectory correction method based on model predictive control according to claim 1, 2, or 3, characterized in that: In step S3, for the disturbance term, the first objective function is minimized. Optimize matrix A, matrix B, and perturbation term. ,in, This is the trajectory error model. Let k be the robot's actual pose and velocity at time k. For the robot's predicted pose and velocity at time k, the perturbation term... This corresponds to external disturbance data.
6. The robot filament placement trajectory correction method based on model predictive control according to claim 5, characterized in that: In step S3, for the trajectory error model, the trajectory error is first calculated based on the robot's execution trajectory and the reference trajectory, and then the trajectory error is fitted by spline interpolation based on the first layer processing data to form a trajectory error model that is represented as a continuous disturbance curve.
7. The robot filament placement trajectory correction method based on model predictive control according to claim 6, characterized in that: In step S3, the optimization matrix A, matrix B, and perturbation term are optimized. Then, by minimizing the second objective function Optimize the weight matrices Q and R, where x ref,k+i Let k+i be the reference pose and velocity. , For the weighted average sum, This indicates that the pose and velocity at time k+i are predicted based on information from time k. This represents the control input for predicting time k+i based on information from time k.
8. The robot filament placement trajectory correction method based on model predictive control according to claim 7, characterized in that: In step S4, the OSQP solver deployed in the control module of the wire laying CNC system is used to perform optimization calculations for the predictive controller.