Safe PID Controller for Two Link Robot using High-Order Control Barrier Function

Since R2026a

This example uses:

This example shows how to implement safety constraints using the High-Order Control Barrier Function block for a two-link robot with PID controller. The two-Link robot model and its corresponding dynamics are based on [1] and [2].

Nonlinear Model and Objective

In this example, consider a two-link robot. The control objective is to move the robot tip from its starting point to a defined end point in the planar 2-D $x$ - $y$ space. The tip position also has state constraints that you must consider when designing the controller. This figure shows the schematic of the two-link planar robot.

The robot configuration is defined by the joint angles $θ_{1}$ , $θ_{2}$ and the torques $τ_{1}$ and $τ_{2}$ applied at the joints to control the tip position. The state vector is:

$ζ (t) = [\begin{array}{c} θ_{1} \\ θ_{2} \\ {θ_{}^{˙}}_{1} (t) \\ {θ_{}^{˙}}_{2} (t) \end{array}]$ .

The following equations describe the robot dynamics.

$[\begin{array}{cc} α + 2 β \cos (θ_{2}) & δ + β \cos (θ_{2}) \\ δ + β \cos (θ_{2}) & δ \end{array}] [\begin{array}{c} {θ_{1}}_{}^{¨} \\ {θ_{2}}_{}^{¨} \end{array}] + [\begin{array}{cc} - β \sin (θ_{2}) {θ_{2}}_{}^{˙} & β \sin (θ_{2}) ({θ_{1}}_{}^{˙} + {θ_{2}}_{}^{˙}) \\ β \sin (θ_{2}) {θ_{1}}_{}^{˙} & 0 \end{array}] [\begin{array}{c} {θ_{1}}_{}^{˙} \\ {θ_{2}}_{}^{˙} \end{array}] = [\begin{array}{c} τ_{1} \\ τ_{2} \end{array}]$

Here:

$α = I_{z1} + I_{z2} + m_{1} r_{1}^{2} + m_{2} (l_{1}^{2} + r_{2}^{2})$

$β = m_{2} l_{1} r_{2}$

$δ = I_{z2} + m_{2} r_{2}^{2}$

Define the physical parameters.

m1 = 3;         % Mass of link 1, kg
m2 = 2;         % Mass of link 2, kg
L1 = 0.3;       % Length of link 1, meters
L2 = 0.3;       % Length of link 2, meters

Calculate inertia terms for the Simulink® model.

r1 = L1/2;      % Length to centroid of link 1, meters
r2 = L2/2;      % Length to centroid of link 2, meters
Iz1 = m1*L1^2/12; % Moment of inertia of link 1, kg*m^2
Iz2 = m2*L2^2/12; % Moment of inertia of link 2, kg*m^2
beta = m2*L1*r2;
delta = Iz2 + m2*r2^2;
alpha = Iz1 + Iz2 + m1*r1^2 + m2*(L1^2 + r2^2);

The $x$ - $y$ coordinates of the endpoints of link 1 and link 2, as functions of the link angles $[θ_{1}, θ_{2}]$ , are defined as follows:

Position of link 1 — $\begin{array}{l} x_{1} = L_{1} \cos (θ_{1}) \\ y_{1} = L_{1} \sin (θ_{1}) \end{array}$

Position of link 2 — $\begin{array}{l} x_{2} = x_{1} + L_{2} \cos (θ_{1} + θ_{2}) \\ y_{2} = y_{1} + L_{2} \sin (θ_{1} + θ_{2}) \end{array}$

PID Controller for Two-Link Robot

For state-feedback control of the two-link robot, convert the start and end tip positions to joint angles using inverse kinematics for the two-link arm. Use the provided twoLinkInverseKinematics.m script to obtain joint angles from tip coordinates.

Specify the start position of tip of two-link robot as $[x_{2} (0), y_{2} (0)]$ to obtain the corresponding link angles $[θ_{1} (0), θ_{2} (0)]$ .

InitialX = -0.5;
InitialY = -0.0496;
[InitialTheta1ref, InitialTheta2ref] = twoLinkInverseKinematics(InitialX,InitialY,L1,L2);

Similarly, specify the end position $[x_{2} (t_{f}), y_{2} (t_{f})]$ to obtain corresponding link angles $[θ_{1} (t_{f}), θ_{2} (t_{f})]$ .

FinalX = 0.5;
FinalY = 0.2496;
[FinalTheta1ref, FinalTheta2ref] = twoLinkInverseKinematics(FinalX,FinalY,L1,L2);

Design the PID controllers to move the two-link robot from its initial orientation to the final orientation of the links. Implement the PID controllers as two decoupled channels. One for controlling angle $θ_{1}$ of link 1, the other angle $θ_{2}$ of link 2, as shown in the following figure.

This example uses two PID controllers with gains tuned using the PID Tuner app. For more information on tuning PID controllers for Simulink® models, see Introduction to Model-Based PID Tuning in Simulink.

Results with Baseline PID

Open the model TwoLinkRobotWithPID, which includes the designed PID controllers for the two-link robot.

mdl = 'TwoLinkRobotWithPID';
open_system(mdl);

Simulate the model and plot the PID controller tracking performance.

out = sim(mdl);

% Extract the link angles and control signal
TwoLinkTheta1 = squeeze(out.theta1.Data);
TwoLinkTheta2 = squeeze(out.theta2.Data);
u_pid = out.u.data;

% Plot Link angle
figure
subplot(211)
hold on
plot(out.tout, TwoLinkTheta1,'LineWidth',2)
plot(out.tout, FinalTheta1ref*ones(1,length(out.tout)), "LineWidth",2,'LineStyle','--')
legend('Theta1','Theta1_ref')
ylabel('theta1')
title('Two-Link Robot Angles vs Ref Angle')
subplot(212)
hold on
plot(out.tout, TwoLinkTheta2,'LineWidth',2)
plot(out.tout, FinalTheta2ref*ones(1,length(out.tout)), "LineWidth",2,'LineStyle','--')
legend('Theta2','Theta2_ref')
xlabel('time(sec)')
ylabel('theta2')

Figure contains 2 axes objects. Axes object 1 with title Two-Link Robot Angles vs Ref Angle, ylabel theta1 contains 2 objects of type line. These objects represent Theta1, Theta1_ref. Axes object 2 with xlabel time(sec), ylabel theta2 contains 2 objects of type line. These objects represent Theta2, Theta2_ref.

Plot the PID controller output.

% Plot PID control torques
figure
subplot(211)
plot(out.tout, u_pid(:,1),'LineWidth',2)
ylabel('Torque \tau_1')
title('PID Control')
subplot(212)
plot(out.tout, u_pid(:,2),'LineWidth',2)
xlabel('time(sec)')
ylabel('Torque \tau_2')

$Figure contains 2 axes objects. Axes object 1 with title PID Control, ylabel Torque \tau_1 contains an object of type line. Axes object 2 with xlabel time(sec), ylabel Torque \tau_2 contains an object of type line.$

View the animation of the two-link robot under PID control as it moves from the specified start position to the end position.

PlotTwoLinkArm(TwoLinkTheta1,TwoLinkTheta2,InitialX,InitialY,FinalX,FinalY,L1,L2,-0.4)

Figure contains an axes object. The axes object contains 4 objects of type scatter, line, animatedline.

The standard PID controller does not provide a way to enforce safety constraints. The next section introduces control barrier functions, which add safety constraints to the baseline controller.

Safe Controller Using Control Barrier Function

A control barrier function (CBF) is a mathematical method that guarantees safety of a controlled system by ensuring the state remains within a defined safe region. It achieves this by imposing constraints on the control input at each time step, preventing the system from entering unsafe regions [3].

To introduce optimization-based safe controllers using CBF, consider a nonlinear, control-affine system defined as follows:

$x_{}^{˙} = f (x) + g (x) u$ .

For the given dynamical system and in the context of CBF, define a safe set $C$ as the set of all state values $x (t)$ that satisfy the condition $h (x) \geq 0$ . Here, the smooth function $h (x)$ is the control barrier function.

You can construct a control barrier function $h (x)$ such that a state trajectory $x (t) \forall t > 0$ that remains within the set $C : h (x) > 0$ is considered safe.

Given the control system and the safe set $C$ defined by function $h (x)$ , the safety condition is defined as follows:

For all control $u (t)$ , ensure $h_{}^{˙} (x, u) \geq - γ h (x)$ .

Here, $γ$ is an extended class $K_{\infty}$ function, and $h_{}^{˙} (x, u)$ is the Lie derivative. For more details, see Enforce Safety Constraints with Control Barrier Functions.

Safe Control Synthesis Using CBF

Using the definition of the safe set, you can derive the safe control through an optimization routine.

Before that, review the concept of the Lie derivative of a CBF $h (x)$ . The CBF is scalar function such that $h (x) : R^{n} \to R$ . The Lie derivative is the time derivative of $h (x)$ along the flow field given by the system dynamics $x_{}^{˙} = f (x) + g (x) u$ .

The Lie derivative is defined as:

$h_{}^{˙} (x, u) = \frac{\partial h (x)}{\partial x} x_{}^{˙}$ .

Using the definition of the system dynamics, you can further simplify the Lie derivative as:

$h_{}^{˙} (x, u) = \frac{\partial h (x)}{\partial x} f (x) + \frac{\partial h (x)}{\partial x} g (x) u$ .

Hence, the safety condition is written as:

$L_{f} h (x) + L_{g} h (x) u \geq - γ h (x)$ ,

where $L_{f} h (x) = \frac{\partial h (x)}{\partial x} f (x)$ and $L_{g} h (x) = \frac{\partial h (x)}{\partial x} g (x)$

Using the defined inequality constraint, you can now compute the safe control $u^{*}$ . To achieve this, consider the following Quadratic Programming (QP)-based controller that finds the minimum perturbation on $u$ as follows:

$\begin{array}{l} u^{*} = \min_{u} \frac{1}{2} | | u - u_{0} | |^{2} \\ s . t L_{f} h (x) + L_{g} h (x) u \geq - γ h (x), \end{array}$

where $u_{0}$ is any designed baseline controller.

When the control term does not appear in the first-order Lie derivative of the CBF, you can use higher-order derivatives of the CBF. For the theory and derivation of high-order barrier functions, see [3] and Enforce Safety Constraints with Control Barrier Functions.

High-Order CBF for Two-Link Robot

For the two-link robot problem, you impose a safety constraint that prevents the tip of the second link from extending beyond the specified boundary $y_{2} = - 0.4$ as shown in this figure.

The control barrier function for the corresponding safety constraint is mathematically defined as:

$h (y_{2}) = 0.4 + y_{2} (t)$ .

This barrier function models the safe and unsafe regions as follows:

$C = {\begin{array}{ll} \infty > y_{2} > - 0.4 & : h (y_{2}) > 0 (Safe) \\ y_{2} = - 0.4 & : h (y_{2}) = 0 (Safe boundary) \\ y_{2} < - 0.4 & : h (y_{2}) < 0 (Unsafe) \end{array}$

Using the Cartesian coordinates of the endpoint of the two-link robot, the control barrier function is expressed as:

$h (ζ) = 0.4 + {L_{1} \sin (θ_{1}) + L}_{2} \sin (θ_{1} + θ_{2})$

Using the control barrier function theory from the previous section, derive the Lie derivative of CBF and form the QP problem for safe control synthesis.

The constraint equation for safe control is $h_{}^{˙} (ζ, u) \geq - γ h (ζ)$ .

Taking the first Lie derivative of the barrier function with respect to states of two-link robot

$\begin{array}{l} h_{}^{˙} (ζ, u) = \frac{\partial h}{\partial ζ} ζ_{}^{˙} \\ h_{}^{˙} (ζ, u) = L_{f} h = [\begin{array}{cccc} {L_{1} \cos (θ_{1}) + L}_{2} \cos (θ_{1} + θ_{2}) & L_{2} \cos (θ_{1} + θ_{2}) & 0 & 0 \end{array}] [\begin{array}{c} {θ_{1}}_{}^{˙} \\ {θ_{2}}_{}^{˙} \\ {θ_{1}}_{}^{¨} \\ {θ_{2}}_{}^{¨} \end{array}] \end{array}$

As you can see the first Lie derivative does not expose the control term $u (t)$ . For safety constraint we require the control to appear linearly in the constraint equation. Hence we proceed to take the second Lie derivative as follows

$\begin{array}{l} h_{}^{¨} (ζ, u) = \frac{\partial L_{f} h}{\partial ζ} ζ_{}^{˙} \\ h_{}^{¨} (ζ, u) = [\begin{array}{cccc} - {θ_{1}}_{}^{˙} L_{1} \sin (θ_{1}) - {{θ_{1}}_{}^{˙} L}_{2} \sin (θ_{1} + θ_{2}) - {{θ_{2}}_{}^{˙} L}_{2} \sin (θ_{1} + θ_{2}) & - {{θ_{1}}_{}^{˙} L}_{2} \sin (θ_{1} + θ_{2}) - {{θ_{2}}_{}^{˙} L}_{2} \sin (θ_{1} + θ_{2}) & {L_{1} \cos (θ_{1}) + L}_{2} \cos (θ_{1} + θ_{2}) & L_{2} \cos (θ_{1} + θ_{2}) \end{array}] [\begin{array}{c} {θ_{1}}_{}^{˙} \\ {θ_{2}}_{}^{˙} \\ {θ_{1}}_{}^{¨} \\ {θ_{2}}_{}^{¨} \end{array}] \end{array}$

The second Lie derivative involves term $[{θ_{1}}_{}^{¨}, {θ_{2}}_{}^{¨}]$ hence the control term appears through system dynamics as follows,

$h_{}^{¨} (ζ, u) = L_{f}^{2} h + L_{g} L_{f} h u$

where $L_{f}^{2} h = \frac{\partial L_{f} h}{\partial ζ} f (ζ)$ and $L_{g} L_{f} h = \frac{\partial L_{f} h}{\partial ζ} g (ζ)$

And the plant model $f (ζ)$ and $g (ζ)$ are defined as follows

$f (ζ) = [\begin{array}{cc} I_{2 \times 2} & 0_{2 \times 2} \\ 0_{2 \times 2} & M (ζ) \end{array}]$ and $g (ζ) = [\begin{array}{c} 0_{2 \times 2} \\ G (ζ) \end{array}]$

$M (ζ) = {[\begin{array}{cc} α + 2 β \cos (θ_{2}) & δ + β \cos (θ_{2}) \\ δ + β \cos (θ_{2}) & δ \end{array}]}^{- 1} [\begin{array}{cc} - β \sin (θ_{2}) {θ_{2}}_{}^{˙} & β \sin (θ_{2}) ({θ_{1}}_{}^{˙} + {θ_{2}}_{}^{˙}) \\ β \sin (θ_{2}) {θ_{1}}_{}^{˙} & 0 \end{array}]$ , $G (ζ) = {[\begin{array}{cc} α + 2 β \cos (θ_{2}) & δ + β \cos (θ_{2}) \\ δ + β \cos (θ_{2}) & δ \end{array}]}^{- 1}$ and $u = [\begin{array}{c} τ_{1} \\ τ_{2} \end{array}]$

The second-order CBF safety constraints and QP formulation for the two-link robot are defined as:

$\begin{array}{l} u^{*} = \min_{u} \frac{1}{2} {‖ u - u_{pid} ‖}^{2} \\ s . t L_{f}^{2} h + L_{g} L_{f} h u + γ_{1} h + γ_{2} L_{f} h \geq 0 \end{array}$

You can implement this derived formulation using the High-Order Control Barrier Function block, as shown in the next section.

High-Order CBF Results

The TwoLinkRobotWithHOCBF model contains the PID controllers, the plant dynamics, and the high-order control barrier function implementation.

mdl = 'TwoLinkRobotWithHOCBF';
open_system(mdl);

To view the CBF implementation, open the Control Barrier Function subsystem. The model implements the known CBF using a MATLAB Function block, and the High-Order Control Barrier Function block enforces the constraint function.

Simulate the model and plot the results.

out = sim(mdl);

Extract the link angles and control signal.

TwoLinkTheta1 = squeeze(out.theta1.Data);
TwoLinkTheta2 = squeeze(out.theta2.Data);
u_hocbf = squeeze(out.ustar.data);
u_pid = squeeze(out.upid.data);
% Plot Link angle
figure
subplot(211)
hold on
plot(out.tout, TwoLinkTheta1,'LineWidth',2)
plot(out.tout, FinalTheta1ref*ones(1,length(out.tout)), "LineWidth",2,'LineStyle','--')
legend('Theta1','Theta1_ref')
ylabel('theta1')
title('Two-Link Robot Angles vs Ref Angle')
subplot(212)
hold on
plot(out.tout, TwoLinkTheta2,'LineWidth',2)
plot(out.tout, FinalTheta2ref*ones(1,length(out.tout)), "LineWidth",2,'LineStyle','--')
legend('Theta2','Theta2_ref')
xlabel('time(sec)')
ylabel('theta2')

Compare the high-order CBF constrained control with the baseline PID control.

% Plot PID control torques
figure
subplot(221)
plot(out.tout, u_pid(1,:),'LineWidth',2)
ylabel('PID Torque \tau_1')
title('PID control of two-link robot')
subplot(222)
plot(out.tout, u_hocbf(:,1),'LineWidth',2)
ylabel('HO-CBF Torque \tau_1')
title('Safe control of two-link robot')
subplot(223)
plot(out.tout, u_pid(2,:),'LineWidth',2)
ylabel('PID Torque \tau_2')
xlabel('time(sec)')
subplot(224)
plot(out.tout, u_hocbf(:,2),'LineWidth',2)
ylabel('HO-CBF Torque \tau_2')
xlabel('time(sec)')

$Figure contains 4 axes objects. Axes object 1 with title PID control of two-link robot, ylabel PID Torque \tau_1 contains an object of type line. Axes object 2 with title Safe control of two-link robot, ylabel HO-CBF Torque \tau_1 contains an object of type line. Axes object 3 with xlabel time(sec), ylabel PID Torque \tau_2 contains an object of type line. Axes object 4 with xlabel time(sec), ylabel HO-CBF Torque \tau_2 contains an object of type line.$

View the animation of the two-link robot under safe control, generated by the high-order CBF with the baseline PID control, as it moves from the specified start position to the end position.

PlotTwoLinkArm(TwoLinkTheta1,TwoLinkTheta2,InitialX,InitialY,FinalX,FinalY,L1,L2,-0.4)

Figure contains an axes object. The axes object contains 4 objects of type scatter, line, animatedline.

The High-Order Control Barrier Function block successfully constrains the tip of the two-link robot within the given safe state bounds.

bdclose(TwoLinkRobotWithPID)
bdclose(TwoLinkRobotWithHOCBF)

References

[1] Murray, Richard M., Zexiang Li, and S. Shankar Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994.

[2] Seiler, Peter, Robert M. Moore, Chris Meissen, Murat Arcak, and Andrew Packard. “Finite Horizon Robustness Analysis of LTV Systems Using Integral Quadratic Constraints.” Automatica 100 (February 2019): 135–43. https://doi.org/10.1016/j.automatica.2018.11.009.

[3] Xiao, Wei, and Calin Belta. “High-Order Control Barrier Functions.” IEEE Transactions on Automatic Control 67, no. 7 (2022): 3655–62. https://doi.org/10.1109/TAC.2021.3105491.