A robot arm predictive control method

Abstract

The present application relates to the technical field of mechanical arm control, and especially relates to a kind of mechanical arm prediction control method, the method comprises the following steps: defining mechanical arm state and modeling;Data acquisition constructs Koopman linear promotion deterministic prediction model;Error is probabilistically characterized using Gaussian process regression;Combining deterministic model and error model constructs random prediction model, and derives state distribution evolution equation;Joint constraint is converted into probability opportunity constraint and is determined equivalence conversion, and the rolling optimization problem is solved.The present application can abandon the dependence on accurate dynamics model, and realizes accurate approximation to nonlinear dynamics by data driving, realizes high-precision trajectory tracking of mechanical arm end under the premise of guaranteeing high probability to meet safety constraint, and reduces control conservatism.

Description

Technical Field

[0001] This invention relates to the field of robotic arm control technology, and in particular to a predictive control method for robotic arms. Background Technology

[0002] With the continuous development of intelligent manufacturing, medical surgery, and special operations, the task complexity of robotic arm systems is increasing, placing higher demands on the trajectory tracking accuracy, response speed, and safety constraint compliance capabilities of end effectors. Trajectory planning, as a core component of robotic arm control, aims to generate a smooth and executable end-effector trajectory while satisfying joint physical constraints and avoiding singular configurations and obstacle collisions. Traditional trajectory planning methods are typically based on accurate kinematic or dynamic models of the robotic arm and employ optimization or feedback control strategies. However, real-world robotic arm systems commonly exhibit various uncertainties, such as unmodeled dynamics, parameter uncertainties, joint friction, external disturbances, and sensor noise. These uncertainties lead to performance degradation of nominal model-based planning methods in practical applications and may even cause safety issues.

[0003] To address the reliance on precise models, data-driven control methods based on the Koopman operator offer a novel approach to predictive control without explicit models. This theory maps nonlinear systems to a high-dimensional linear space and utilizes linear system theory for prediction and control design. However, existing data-driven control methods still have significant limitations when dealing with complex robotic arm systems. First, the model prediction accuracy is limited, especially as the prediction time domain increases, leading to severe accumulation of multi-step prediction errors. Second, existing methods are insufficient in handling modeling errors and external disturbances, often simplifying them into random variables independent of system state or control input, which clearly does not reflect the characteristics of real-world systems. Furthermore, under strict physical constraints such as joint velocities and accelerations, existing methods struggle to effectively optimize trajectory tracking performance while ensuring system safety, and constraint handling is often overly conservative.

[0004] Addressing the challenges of existing model-based robotic arm trajectory planning methods, such as their strong reliance on precise models and insufficient robustness, as well as the large modeling errors and difficulties in constraint handling in existing data-driven methods, developing a novel control scheme that integrates the advantages of probabilistic representation and nonlinear mapping has significant scientific and practical value. By constructing a linear lifting prediction model containing uncertainties using limited robotic arm input and output data, and introducing Gaussian process regression to probabilistically compensate for model errors, high-precision and high-reliability trajectory tracking control of the robotic arm's end effector can be achieved under strict compliance with joint physical probability constraints. This effectively improves the intelligence level and operational safety of the robotic arm in complex working environments. Summary of the Invention

[0005] This invention addresses the shortcomings of existing robotic arm trajectory planning and control technologies, such as strong model dependence, insufficient accuracy of data-driven modeling, and overly conservative constraint handling. It proposes a robotic arm predictive control method based on the fusion of Gaussian process regression and the Koopman operator. This method abandons the reliance on precise dynamic models, constructs a linear boosting predictive model with probabilistic error compensation capabilities using limited operational data, and establishes a probabilistic chance constraint optimization framework that satisfies joint physical constraints. This achieves high-precision end-effector trajectory tracking while ensuring system safety.

[0006] This invention provides a predictive control method for robotic arms, whose overall architecture includes five core components: system state definition, Koopman linear lifting model construction, Gaussian process regression error modeling, stochastic state distribution evolution derivation, and probabilistic constraint transformation and rolling optimization control. The method does not rely on the robotic arm's mass, inertia, friction coefficient, or other dynamic parameters; it only requires collecting the robotic arm's input and output data within its safe operating range to complete model training and controller design. It is suitable for complex operating environments with unmodeled dynamics, external disturbances, and parameter uncertainties.

[0007] S1: System State Definition and Discretization Modeling. Consider a system with... A robotic arm system with three degrees of freedom is defined as follows: the three-dimensional position vector of the end effector in the base coordinate system is... Its attitude is represented by Euler angles. Define the joint angle vector as The joint angular velocity vector is Therefore, the system state vector is constructed. Control input Defined as joint angular acceleration command In the discrete time domain, with a fixed sampling period Discretizing the continuous system yields the discrete-time kinematic model of the robotic arm: ,in Represents the unknown dynamics of the real system. To integrate uncertainties, this model encompasses unmodeled dynamics, external disturbances, sensor noise, and discretization errors. As the foundation for subsequent data-driven modeling, its structure does not rely on any prior dynamic knowledge.

[0008] S2: Construct a Koopman linear lifting deterministic prediction model. Data is collected within the safe operating area of ​​the robotic arm. The input and output data constitute an offline dataset. ,in Design nonlinear mapping functions. Mapping the original state space to a higher-dimensional boosted space. To enhance dimensions and satisfy The system matrix of the linear lifting system is obtained by solving the following least-squares optimization problem. With input matrix :

[0009]

[0010] Meanwhile, by solving the output mapping optimization problem Obtain the output matrix This is used to restore the original state from the boosted state. The above optimization problem has an analytical solution, which can be obtained directly through matrix pseudo-inverse operations. Therefore, a deterministic linear boosting prediction model is established:

[0011] ,

[0012]

[0013] in To elevate the status, the symbol " "Indicates the current moment" right The predicted state at time step. This model approximates the original nonlinear system as a high-dimensional linear system, laying the foundation for the subsequent introduction of probabilistic error compensation.

[0014] S3: Probabilistic representation of modeling error based on Gaussian process regression. Since the approximation of the Koopman operator in finite-dimensional space inevitably introduces modeling error, this error is defined as... , where the extended input vector To accurately characterize the statistical properties of this error as it varies with system state and control input, the error vector is... Each dimension component ( Establish independent Gaussian process models for each process. Preset the covariance function (kernel function). Optional forms include the squared exponential kernel function or the thin-plate radial basis spline function. Based on an offline dataset. Calculate error samples ,in , Each is a matrix , The Okay. Offline optimization of the kernel function hyperparameters is performed by maximizing the marginal likelihood function. For any new input... The posterior distribution of the corresponding error follows a Gaussian distribution:

[0015]

[0016] The mean function covariance matrix Each component is calculated using the standard Gaussian process regression formula. This step enables state-control dependent probabilistic modeling of the residual error of the Koopman model, significantly improving the model's ability to approximate nonlinear dynamics.

[0017] S4: Constructing the stochastic prediction model and state distribution evolution. The deterministic Koopman model from S2 and the Gaussian process error model from S3 are combined to construct a complete stochastic prediction model:

[0018]

[0019] because For random variables, promote state This also becomes a stochastic process. To facilitate online optimization calculations, the mean equivalent approximation method of first-order moment matching is adopted to derive the lifting state mean. With covariance The recursive evolution equation is as follows. Specifically, the mean value evolution equation is:

[0020]

[0021] The covariance evolution equation is:

[0022]

[0023] Initial conditions are set as follows , This means that the current state is known and there is no uncertainty. Through the above recursive relationship, the mean and covariance of the state distribution can be propagated step by step in the prediction time domain, providing the necessary statistical information for subsequent probability constraint transformation.

[0024] S5: Probabilistic Constraint Transformation and Stochastic Model Predictive Control Solution. The physical constraints of the robotic arm joints (such as joint angle limits and angular velocity limits) are expressed as probabilistic chance constraints. Taking joint angular velocity constraints as an example, the requirements are... The boosted state distribution obtained from propagation in S4 is used. and output mapping This probabilistic constraint can be transformed into a deterministic equivalent constraint:

[0025]

[0026] in To select a matrix that satisfies That is, extract the corresponding number in the lifting state. A linear combination of joint angular velocities. Similar methods can be applied to other physical constraints such as joint angles and accelerations. Based on this, a finite-time optimization problem is constructed with the goal of minimizing the end-effector position and attitude tracking error:

[0027]

[0028] st

[0029]

[0030]

[0031]

[0032]

[0033] The transformed deterministic constraints

[0034] in To predict the number of steps, and The desired terminal trajectory. At each sampling time... Solve the above nonlinear optimization problem to obtain the optimal control sequence. and the first element The process is applied to the robotic arm system. Then, it rolls to the next moment, repeating the above process to form a closed-loop feedback control, achieving high-precision tracking of the desired trajectory.

[0035] The beneficial effects of this invention are as follows: By integrating the linear lifting capability of the Koopman operator with the nonlinear error compensation mechanism of Gaussian process regression, a data-driven prediction model that combines computational efficiency and modeling accuracy is constructed. This model explicitly learns and compensates for the residual error introduced by the Koopman approximation, and the error is modeled as a function of state and control, effectively suppressing the error accumulation phenomenon in multi-step prediction. Furthermore, by establishing the mean and covariance evolution equations of the random state distribution and transforming the joint physical constraints into deterministic equivalent constraints related to the state covariance, this invention significantly reduces the conservatism of traditional robust control methods while ensuring a high probability of satisfying safety constraints, enabling the robotic arm to still achieve maximum motion performance near the constraint boundaries. In addition, the entire control framework is entirely based on runtime data-driven operation, requiring no prior dynamic parameters, exhibiting good adaptability and cross-platform portability, and is suitable for complex operation scenarios with unknown parameters, strong disturbances, or dynamic changes. Experimental verification shows that this method can achieve high-precision tracking with an end-effector position error of less than 2 mm and an attitude error of less than 1 degree on a six-degree-of-freedom robotic arm, while strictly meeting the probabilistic constraints of joint angles and velocities. Attached Figure Description

[0036] Figure 1 This is a block diagram of the overall architecture of the predictive control method for robotic arms proposed in this invention.

[0037] Figure 2 This is a position tracking effect curve of the end effector of the robotic arm in an embodiment of the present invention;

[0038] Figure 3 This is a graph showing the attitude tracking effect of the robotic arm end effector in an embodiment of the present invention.

[0039] Figure 4 This is a schematic diagram illustrating the evolution trajectory of the robotic arm joint angles and the satisfaction of probabilistic constraints in an embodiment of the present invention.

[0040] Figure 5 This is a schematic diagram illustrating the evolution trajectory of the joint angular velocity of the robotic arm and the satisfaction of probabilistic constraints in an embodiment of the present invention. Detailed Implementation

[0041] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0042] This embodiment provides a predictive control method for a robotic arm. Its core logic lies in using high-dimensional linear space mapping and probabilistic machine learning to address the high dependence of the robotic arm on precise dynamic models in complex working environments, and effectively address the risks of decreased control accuracy and violation of safety constraints caused by system uncertainties. The method's operation mainly consists of an offline data training phase and an online rolling optimization control phase, the specific operating principles of which are described below.

[0043] S1: System state definition and discretization modeling. Before the robotic arm control task starts, the first step is to define the system state and perform discretization modeling. The physical characteristics of the robotic arm with degrees of freedom are mathematically parameterized. The position vector of the end effector in the base coordinate system is defined as follows. This vector contains three-dimensional spatial coordinate information. Simultaneously, it utilizes Euler angle vectors. The attitude of the end effector is characterized by a vector consisting of roll, pitch, and yaw angles. For the internal joints of the robotic arm, the joint angle vector is defined as follows: Its dimensions and the degrees of freedom of the robotic arm Consistent. To comprehensively characterize the motion state of the robotic arm, a system state vector is constructed. This vector is obtained by concatenating the end-effector position, end-effector pose, joint angle, and joint angular velocity, and its total dimension is [missing information]. Joint angular acceleration It is set as a control input, and indirectly adjusts the joint velocity and angle by controlling angular acceleration. In the discretization process, the sampling period is set to... The continuous time-dynamic evolution process is transformed into a discrete time series, constructing a system containing unknown real dynamic functions. and combined disturbance term The discrete state-space equations. The comprehensive disturbance term here... It covers all factors that are difficult to capture through analytical modeling, such as joint friction, sensor random noise, and environmental interference.

[0044] S2: Offline dataset acquisition and preprocessing. To drive subsequent data-driven modeling, the control system executes a preset excitation trajectory within a safety envelope, and sensors record the robotic arm's operational data in real time. (Acquisition) The group contains the current state. Current control input and the state at the next moment The data pairs constitute the offline dataset. During data acquisition, pseudo-random binary sequence signals or multi-frequency sinusoidal superposition signals are applied to the joints to ensure sufficient excitation intensity of the control input in the frequency domain, thereby enabling the dataset to cover the main nonlinear characteristics of the robotic arm within the workspace. After acquisition, the data undergoes normalization to eliminate the impact of differences in the dimensions of different physical quantities on the convergence speed of subsequent optimization algorithms.

[0045] S3: Constructing a Koopman linear lifting deterministic prediction model. Due to the highly nonlinear characteristics of the robotic arm's kinematics and dynamics, directly performing linear predictions in the low-dimensional original state space would lead to significant modeling errors. This embodiment utilizes Koopman operator theory to design a nonlinear mapping function. , the original state vector Mapped to a In a higher-dimensional lifting space, the lifting state variables are obtained. The mapping function chosen is the thin-plate radial basis spline function. By uniformly distributing center points in the state space, a combination of nonlinear basis functions is used to approximate the infinite-dimensional Koopman operator. This is based on an offline dataset. The linear evolution matrix in the lift space is solved using the least squares method. With input matrix This minimizes the sum of squared Euclidean distances between the mapped predicted state and the actual observed state. Simultaneously, the output matrix is ​​solved. Establish a state of elevation from a higher dimension Return to the original state The linear mapping relationship is established. This step transforms the original nonlinear robotic arm system into a linear controlled system in a high-dimensional space, providing mathematical convenience for subsequent large-scale predictive calculations using linear control theory.

[0046] S4: Modeling Error Probability Representation Based on Gaussian Process Regression. Considering the inevitable residual error in the Koopman boosting model when truncated to finite dimensions, a modeling error vector is defined. This represents the difference between the actual state evolution value and the predicted value from the linear boosting model. To achieve accurate compensation for this error, the error vector... An independent Gaussian process model is built for each dimension. The squared exponential kernel function is chosen as the covariance function, and the hyperparameter vector is used to construct the model. Adjust the smoothness and noise tolerance of the model. Train the hyperparameters using maximum likelihood estimation so that the Gaussian process model can adapt to the current lifting state. and control input The posterior probability distribution of the real-time output error is calculated using the mean function. Sum of covariance functions The two functions are described together, where the mean function represents the amount of compensation for deterministic bias in the prediction, and the covariance function quantifies the degree of uncertainty in the prediction results. In this way, the originally difficult-to-describe modeling error is transformed into a random variable with statistical properties, realizing deep learning and quantification of the unmodeled dynamics of the robotic arm.

[0047] S5: Construction of the Stochastic Prediction Model and Evolution of State Distribution. The deterministic linear model in S3 and the Gaussian process random error model in S4 are nonlinearly superimposed to construct a complete stochastic prediction model. Under this model, the future improved state... Instead of a fixed point, the state is a random variable following a specific distribution. To enable online computation, a first-order moment matching technique is used to derive the recursive equation for the state distribution. Within each prediction step, the mean of the state is increased. Through linear matrix , Update the mean error of the Gaussian process; increase the covariance of the state. The covariance is then linearly transformed from the previous time step and added to the error covariance of the Gaussian process prediction at the current time step. The mean at the initial time step is set as the mapping value of the current sensor feedback state, and the initial covariance is set as a zero matrix. This distributed evolution mechanism enables the controller to perceive the prediction risk in real time within the prediction time domain. As the number of prediction steps increases, the trace of the covariance matrix gradually increases, intuitively reflecting the accumulation process of uncertainty in multi-step prediction.

[0048] S6: Transformation of Probabilistic Constraints. During operation, the robotic arm must strictly adhere to joint angle limits, angular velocity limits, and angular acceleration constraints. To balance operational efficiency and safety, these physical constraints are transformed into probabilistic constraints, meaning the probability of each constraint being satisfied is greater than a preset confidence level. Using the boosted state mean and covariance obtained in S5, combined with the output mapping matrix... The probability density distribution of the original state variables (such as joint velocities) is derived. Based on the Gaussian distribution assumption, the inverse cumulative distribution function of the standard normal distribution is used to transform the probabilistic constraints containing random variables into deterministic nonlinear constraints containing mean and covariance terms. The transformed constraints have significant physical meaning: the mean term must maintain a certain safety margin relative to the constraint boundary, and the magnitude of this margin is proportional to the prediction uncertainty (covariance). When the system operates in a region of high uncertainty, the controller will automatically adopt a more conservative strategy, guiding the robotic arm away from the constraint boundary to ensure system safety.

[0049] S7: Construction and Rolling Solution of the Stochastic Model Predictive Control Problem. The objective function is the weighted sum of the squared errors of the end effector's position and attitude tracking. A penalty term for the rate of change of the control input is added to the objective function to ensure that the generated trajectory is smooth and easy to execute. At each sampling time... The controller receives the current state feedback of the robotic arm and, combined with the evolution equations and constraints in S5 and S6, constructs a nonlinear programming problem in the finite time domain. A high-performance numerical optimization solver is then used to solve this problem in real time, calculating the future... The optimal joint angle acceleration sequence within each prediction step. The controller extracts only the first control variable of this sequence, converting it into a current or torque command and applying it to the servo drive of the robotic arm. Subsequently, the system enters the next sampling cycle, repeating the above process of state acquisition, model prediction, and optimization solution. This rolling optimization mechanism gives the robotic arm a strong real-time correction capability, enabling it to maintain accurate tracking of the end effector trajectory by dynamically adjusting subsequent control sequences when faced with sudden external disturbances.

[0050] To verify the effectiveness of the above method, this embodiment uses a six-DOF robotic arm as the experimental object. The structural parameters of the robotic arm are defined using the DH parameter table, covering geometric information such as link length, offset, and torsion angle. Strict joint angle range and angular velocity limits were set in the experiment, with the angular velocity limit set at 20 degrees per second. Initially, the robotic arm is in a specific joint pose, and the goal is to drive the end effector to move to a position with a three-dimensional coordinate system. meters, attitude angle is The desired pose of degrees.

[0051] See attached document Figure 1 The control framework shown first involves the system using an offline dataset. Complete the Koopman matrix , , And the training of Gaussian process hyperparameters. During online operation, the robotic arm's state feedback... The data is fed into the stochastic prediction model in real time.

[0052] See attached document Figure 2 and attached Figure 3 This demonstrates the changes in the position and attitude of the robotic arm's end effector over time. Driven by the controller, the end effector's three-dimensional position coordinates smoothly converge to the target value, with the position tracking error rapidly decreasing within 2 seconds and eventually stabilizing below 2 millimeters. The evolution of the attitude angles also exhibits excellent convergence characteristics; the roll, pitch, and yaw angles all accurately lock onto the desired values, and the attitude error remains below 1 degree. This indicates that the Koopman model, incorporating Gaussian process compensation, possesses extremely high prediction accuracy and can effectively offset the negative impact of unmodeled dynamics on the end effector's accuracy.

[0053] See attached document Figure 4 and attached Figure 5 This demonstrates the curves showing the changes in joint angle trajectories and angular velocities. Throughout the entire movement, the angles of all joints remain within a preset range. and Within the safe range, especially when the angular velocity approaches the constraint boundary of 20 degrees per second, due to the probabilistic constraint, the controller automatically adjusts the angular acceleration command, resulting in a smooth saturation transition of the angular velocity curve at the boundary, rather than violent oscillations or exceeding the limit. This deterministic equivalent transformation scheme allows the robotic arm to fully utilize its motion performance and respond to the end-effector trajectory command at the fastest speed without violating physical limitations.

[0054] In its specific operating principle, Gaussian process regression acts as a "real-time learner," continuously observing the difference between predicted and actual values ​​and relating this difference to the current state and control input. When the robotic arm moves near certain highly nonlinear singular configurations, the Koopman linear model often fails. At this time, the mean compensation term of the Gaussian process output increases significantly, correcting the prediction bias of the linear model. Simultaneously, due to the sparse data or dynamic complexity in this region, the covariance of the Gaussian process output also increases synchronously. Through the constraint transformation mechanism in S6, the controller is forced to increase its safety margin. This logic of "perceiving uncertainty and actively avoiding risks" is the core advantage of this invention compared to traditional deterministic control methods.

[0055] Furthermore, the distributed stochastic model predictive control framework employed in this embodiment supports decoupling of multi-joint tasks. When computational resources are limited, complex optimization problems can be decomposed into local optimization problems of multiple subsystems. Through communication and coordination between the joint controllers, globally optimal trajectory tracking results can be achieved. This architecture significantly improves the algorithm's operating efficiency on embedded controllers, allowing the sampling period to be compressed to 100 milliseconds or even shorter, meeting the demands of high-speed, precision operations.

[0056] In summary, this embodiment achieves high-performance control of a robotic arm under conditions of unknown model, disturbances, and hard constraints by constructing a data-driven hybrid model of "linear prediction + stochastic compensation" and combining it with probabilistic chance constraint transformation technology. This method does not rely on any prior knowledge of robotic arm dynamics; controller deployment is completed solely by processing input and output data streams. Experimental data demonstrates that this scheme drives the robotic arm's end effector to a preset desired pose and converges the tracking error to a preset accuracy range, while ensuring that all joint physical variables strictly meet safety constraints throughout the entire stroke. This technical solution can be widely applied in fields with stringent requirements for both reliability and accuracy, such as medical surgical robots, flexible assembly robotic arms, and deep space exploration operations.

Claims

1. A predictive control method for a robotic arm, characterized in that, Includes the following steps: S1: Define the state vector of the robotic arm system ,in This represents the three-dimensional position vector of the end effector in the base coordinate system. Let Euler angles represent the terminal attitude. The joint angle vector. The joint angular velocity vector; for continuous systems with a fixed sampling period Discretization yields the discrete-time model: ,in For unknown real-world system dynamics, control input This is a joint angle acceleration command. To encompass the uncertainties, Indicates the first Each discrete sampling time; S2: Based on offline datasets Construct a Koopman linear boost deterministic prediction model, where , Design nonlinear mapping functions Map the original state to There is room for improvement. The linear lifting system matrix is ​​obtained by solving the least squares method. , and output mapping matrix Build a model , ,in ; S3: Define modeling error Construct extended input vector ;right Gaussian process regression models are built for each dimension component, based on offline datasets. Train the kernel function hyperparameters to obtain the mean error function. With covariance function Such that for any new input The posterior distribution of the error satisfies ; S4: Constructing a stochastic prediction model ; The mean of the lifting state distribution is derived using the first-order moment matching method. With covariance The recursive evolution equation: Initial conditions are , ; S5: Represent joint physical constraints as probabilistic chance constraints, utilizing lifting state distribution information and output mapping. This transforms probabilistic chance constraints into deterministic equivalent constraints; it constructs a system based on the end position. with posture Tracking the desired trajectory and A finite-time optimization problem with the objective as follows, at each sampling time... Solving for the optimal control input sequence The first element is then applied to the robotic arm to form a rolling optimization closed-loop control.

2. The predictive control method for a robotic arm as described in claim 1, characterized in that, The nonlinear mapping function It is implemented using thin-plate radial basis spline functions.

3. The predictive control method for a robotic arm as described in claim 2, characterized in that, The center points of the thin-plate radial basis spline function are uniformly arranged in the state space.

4. The predictive control method for a robotic arm as described in claim 1, characterized in that, The Gaussian process regression model uses the squared exponential kernel function as the covariance function.

5. The predictive control method for a robotic arm as described in claim 4, characterized in that, The hyperparameters of the squared exponential kernel function are optimized offline by maximizing the marginal likelihood function.

6. The predictive control method for a robotic arm as described in claim 1, characterized in that, The probability chance constraint is ,in For the probability of allowed constraint violations, For the first Angular velocity of each joint.

7. The predictive control method for a robotic arm as described in claim 6, characterized in that, The probability constraint is expressed by the inverse cumulative distribution function of the standard normal distribution. Transform into deterministic equivalent constraints: ， in To meet The selection matrix.

8. The predictive control method for a robotic arm as described in claim 1, characterized in that, The offline dataset The data is obtained by applying pseudo-random binary sequence signals or multi-frequency sinusoidal superposition signals to the robotic arm's safe operating area.

9. The predictive control method for a robotic arm as described in claim 8, characterized in that, The collected raw data was normalized and then used for model training.

10. The predictive control method for a robotic arm as described in claim 1, characterized in that, The objective function of the optimization problem includes control input. The rate of change penalty term.

Images