- The 7 DOF Franka Emika Panda robot
- Table of Content
- The robotic arm
- Trajectory Tracking
- Perturbations of Dynamical parameters and Adaptive strategies
- Overview of the Results
This 7 DOF Franka Emika Panda robot is equipped with 7 revolute joints, each mounting a torque sensor, and it has a total weight of approximately 18kg, having the possibility to handle payloads up to 3kg.
Figure 1 shows the Franka Emika Panda robot, its geometrical parameters and the used DH convention.
The first step was to model the robot using Matlab Robot Control toolbox.
The robot is defined according to the DH table and its parameter are defined and assigned. This is done in
robot_gen.m
The model is saved as PANDA.
The following paramters are used:
The parameters are found in this online repository while the exact procedure is described here:
The resulting model is:
Once we have the dynamical model properly set up, we can proceed to implement the controllers.
| Pick and Place | Circumference | Helix | |
|---|---|---|---|
| PD | ✔️ | ❌ | ❌ |
| PD with gravity compensation | ✔️ | ❌ | ❌ |
| PID | ✔️ | ❌ | ❌ |
| Computed torque | ✔️ | ✔️ | ✔️ |
| Backstepping | ❌ | ✔️ | ✔️ |
| Pick and Place | Circumference | Helix | |
|---|---|---|---|
| Adaptive Computed Torque | ✔️ | ✔️ | ✔️ |
| Adaptive Backstepping | ❌ | ✔️ | ✔️ |
| Li Slotine | ❌ | ✔️ | ✔️ |
Before I dive into the report, I would line to outline the code structure. In the repository, there are essentially three folders.
- Pick and place: It contains every result for the pick and place simulation. Every controller will be found in
./pick_and_place_results/pickandplace_ (desired controller).m
like:
./pick_and_place_results/pickandplace_computedtorque.m
Most of the results are already stored as (desired controller)_results.mat
Comparative plots for the pick and place scenario can be found in
./pick_and_place_results/plot_results.m
- Non adaptive control
All the results are found in:
./non_adaptive_results/non_adaptive_trajectory_tracking.m
In the file, the following sections are found:
- Choose Trajectory (Helix or Circumference)
- Setup
- Compute Reference Trajectory (Cartesian to joint space)
- Execute the computed trajectory with the robot
- Trajectory Tracking: Computed Torque Method
- Computed Torque Results
- Trajectory Tracking: Backstepping Method
- Backstepping Results
- Comparison
- Adaptive control
All the results are found in:
./adaptive_results/adaptive_trajectory_tracking.m
In this file, another manipulator was chosen, as I encountered difficulties in the computation of the dynamic regressor for the Franka 7 DOF arm (and even for a 5 dof simplyfied version of it). Therefore, I switched to the arm implemented in the robotics toolbox, the KUKA 5 DOF robot.
This file has a similar structure: in the first section the trajectory is computed, then the Computed Torque and Backstepping controller are calculated exactly as before. At the end, the mass of the model is modified and the Adaptive Controllers are implemented.
Each implementation will be specifically addressed in the respective adaptive controller section.
I defined three reference trajectories:
| Pick and Place | Circumference | Helix |
|---|---|---|
 |
 |
 |
Each trajectory will now be discussed in details with the implemented controllers.
Initial Conditions
Final Conditions
Starting from the initial conditions I compute the following quantities in a for loop over a 10s period with time constant 1ms :
- Error, derivate of the error, integral of the error
- Dynamic matrices
- Torque, depending on the controller (PD, PD with G compensation, PID)
And last, accelleration, velocity and displacement resulting from the compute torque applied for a delta t of 1ms.
Tustin integration:
The first and most simple solution to control the end effector to a desired pose is to apply standard independent PD controller to follow references.
The aim of using Proportional-Derivative (PD) controller is to increase the stability of the system by improving control since it has an ability to predict the future error of the system response. In order to avoid effects of the sudden change in the value of the error signal, the derivative is taken from the output response of the system variable instead of the error signal. Therefore, D mode is designed to be proportional to the change of the output variable to prevent the sudden changes occurring in the control output resulting from sudden changes in the error signal.
The error is defined as:
In order to verify the stability at the desired value Direct Lyapunov is used with the following candidate:
This implies that PD control the error converges to 0 if the following hypothesis are true:
The first hypotesis will be analyzed in the simulation, while the second means that only step reference function can be given as input. This is ideal for pick and place.
I start with the following gains and then proceed to tune them for each specific controller:
If the remove the G term from the dynamic equation than both hypothesis are true and the error converges to 0.
If we reinsert gravity one hypothesis is no longer true and we have an error offset in the solution:
It is clear how gravity affects only some joints depending on the configuration of the robot.
We can solve this by compensating the gravity term with the following law:
Proportional-Integral-Derivative (PID) controller has the optimum control dynamics including zero steady state error, fast response (short rise time), no oscillations and higher stability. The necessity of using a derivative gain component in addition to the PI controller is to eliminate the overshoot and the oscillations occurring in the output response of the system.
The initial offset slowly converges back to the right value (null steady state error). This PID doesn't require any knowledge of the gravity load for the manipulator.
It is worth mentioning that tuning the PID to achieve stability is much more difficult than tuning just the P and D components.
We don't have anymore the big error offset on the second and sixth configuration variable. However it doesn't have the big advantage of the PID (zero knowledge about gravity load )
In order to achieve perfect reference tracking, the system should have an in built inner dynamical model that can be used to compute a feedforward action that complements the feedback action seen previously. A control that does this is the following:
Substituting this in the dynamics we obtain:
Essentially, the perfect knowledge of the dynamical model allows for complete linearization and desired pole placement. The trajectory tracking can be solved with arbitrarly fast convergence.
When Noise in dynamical parameters knowledge or friction or other sources of error are introduced It is much more difficult to have stability guarantees for the Computed Torque controller.
In the following results, it is clear how deeply even a random error 
A more formal and correct approach would be to apply a control law with estimated M, G and C:
Substituting the dynamics:
Where
The image shows the PD, compensated PD, PID, Computed Torque, and reference signals for the pick and place task.
The integral component can compensate for some of the limits of the PD independent controller, however it was extremely difficult to achieve a good tuning.
The Computed Torque method allows desired pole placement and performances, however requires the exact dynamical model to linearize the dynamic.
The computer Torque method is only robust up to a certain error margin in the estimation of the dynamical parameters.
Given a trajectory in the cartesian space, I compute the references in the joint space with the following block:
I use the non weighted pseudoinverse, therefore the solution will minimize the norm of the q.
Now I will proceed to define the used trajectories and the results for each controller.
- Trajectory tracking I: Circumference
- Trajectory Trackin II: Helix
Considering a system written in the form:
The backstepping controller assumes that a controller exists for the higher level system in the form of a stabilizing feedback law:
The control for the second variable is computed so that it tends to track u’. This is done
For a completely actuated system the backstepping controller is reviewd in the follwing scheme:
First, the necessary quantities are defined (reference velocity, tracking error). Then, the torque is defined as:
I will now investigate what happens if the speed of the reference trajectory is increased. The equation now is:
Which correpsonds to the following joint trajectory:
With the computed torque method, it is really simple to find a good set of paramteres to have really good controller performances:
Parameters for CT control:
|
The controllers provide very good performances despite of the speed of the reference trajectory.
We have demonstrated that most of these controllers are based on a strong assumption, perfect knowledge. This is never the case. In the next example it is clear how just introducing a light friction in the robotic arm causes troubles for the computed torque controller:
Usually, a perfect knowledge of the dynamical matrices is not known, and the controllers must rely on estimates. In such a case,
In the following section some dynamical parameters (the masses) are increased up to 2.5%.
for j = 1:num_of_joints % for each joint/link
KUKAmodel.links(j).m = KUKAmodel.links(j).m .* (1+randi(5,1)/100); %masse [1x1]
end
Central to every adaptive controller is the Dynamic Regressor formulation that allows for a formulation that groups all the uncertainties in the estimation of the dynamic parameters.
The dynamic regressor is a matrix function employed to write the dynamics linearly in the inertial parameters:
The Dynamic regressor an be computed in closed form from the Lagrangian formulation. The lagrangian of a serial chain is defined as:
The equations of motion will be:
Reformulating the dynamics with the regressor gives:
Rerranging the terms:
The regressor has been computed in Wolfram Mathematica 12, using the Screw Calclus Package. The computation of the regressor for the Kinova 7DOF manipulator revealed to be too expensive, therefore I proceeded to compute the regressor for a simplyfied version of the robot (removing the last two links). The computation for the 5 DOF arm infact was the limit in terms of the sheer size of the output that mathematica could handle.
From now on I will refer to this simplyfied model.
- Trajectory I
THe first trajectory is not expected to be sufficiently rich (eg. to excite all the joints ) to be able to estimate all dynamical parameters correctly. (The first joint remains constant)
The second trajectory will excite all the joints. Further experiment must be conducted to understand if it is "sufficiently rich"
- Trajectory II
If there is parameters uncertainties, the torque will be:
Where the dynamic matrices represented above are the estimates based on the uncertainties in the dynamical system. Substituting the torque in the dynamics gives:
The estimated M matrix is considered inverible. As anticipated in the traditional computed torque section, the error dynamics is:
A proper dynamic of the dynamical estimation parameters must be chosen in order to grant the convergence to zero. If we write the error dynamics in normal form:
and the Lyapunov Function will be:
The derivative of the Lyapunov function will be:
and the dynamics of the parameters that makes it n.d is:
The convergence of the error dynamics is not guaranteed unless the Persistent Excitation condition is met:
In this case:
The problems for this control are the invertibility of the estimated Mass matrix and the need for joint accelleration measurements.
This controller has a lot of margin for improvements by tuning the gains for both the error and the dynamic parameters dynamics.
The Adaptive computed torque method was the first adaptive method to be used on robotics manipulator. It is surely an important mileston but suffers some limits:
-
The Estimated Mass matrix is not always invertible.
-
It is usually very difficult to get clean acceleration measurements.
This problems will be solved in the Li-Slotine and Backstepping controllers.
The Li Slotine control uses the reference velocity:
where:
The control torque is:
Deriving the Lyapunov Function:
and substituting the torque we have:
Choosing the following dynamics for the dynamical paremeters estimation:
A s.n.d derivative is obtained:
Using the Barbalat Lemma the asymptotic stability is achieved.
Dynamical parameters estimation is decent: for joints 3,4,5 the mass seems to converge to the right value, while for joints 1 and 2 it doesn't. This could depend from the trajectory which has not sufficient information to make the parameters estimation converges for each joint. The R matrix can surely be tuned more to achieve better estimation, but the simulation are computationally expensive so limited trials were made.
All the results are stored in:
adaptive_backstepping.mat
adaptive_backstepping_dyn_pars.mat
computed_torque.mat
computed_torque_dyn_pars.mat
li-slotine.mat
li-slotine_dyn_pars.mat
We have shown how adaptive controllers are fundamentals for robust control of the robotics manipulator in the presence of uncertainties in the dynamical or friction parameters. These are examples of how fast the computed torque performances degragadates in the presence of noise in the knowledge of the system parameters.
The Li Slotine and the Backstepping Adaptive Controller solve this issues, help to estimate the dynamical parameters and are able to control the system even with significative uncertainties.
In terms of performances all the adpative controllers provide very good tracking error if tuned properly. The dynamical estimation of the parameters is where they differ the most. The following comparison is provided:
The adaptive computed torque has the fastest dynamics and is the most precise if tuned properly (In the figure joint 2 and 3 were not tuned properly) The Backstepping controller is more precise than the Li-Slotine controller, with less oscillation and more steady behaviour. Both however have quite similar performances relative to the computed torque approach.
Adaptive controllers computation required significantly more computational burden than non adpative simulations. This is a hint on how difficult it can be to robustly control manipulators in real time in the presence of uncertanties in the dynamical model.
However, very good parameteric estimation can be made and Symbolic regressors can be built by performing experiments where the required quantities are measured (torques and control inputs). Doing an offline parametric identification can lead to the creation of an accurate model. The main limitation for such techniques is that accurate torque measurements are not always available. This is why the Kinova 7 DOF arm, with its force torque sensors at the joints, provides superior capabilities and allows for high performance control, and this is why it is being widely adopted in the industry.