Head Control

Simplified to a simple pendulum.

You can see the example write in Matlab at the end of this page, or write in Webots with C++ at Webots-Controller:head_control.

args

  • m: mass of the head

  • ω(t): angular velocity of the head

  • I: inertia of the head at the joint

  • u(t): torque applied to the head

  • b=0.001: damping coefficient of the head pitch joint

  • g=9.81: gravity

  • : posture of the body in absolute space coordinates through the gyroscope

  • : posture of the head in absolute space coordinates

  • : angle of the joints

Forward Kinematics

Calculate the inertia matrix

: inertia of the head at the mass center

: Position of the head (xyz).

: Position of the pitch joint (xyz).


Equation:


Output in example:Webots-Controller

Inertial:
   0.0026313 -8.78814e-06  2.99579e-05
-8.78814e-06   0.00249037 -5.33702e-06
 2.99579e-05 -5.33702e-06  0.000984976
Position:
0
0
0
Mass:
0.60533
Head pitch inertia: 0.00249037
Head yaw inertia: 0.000984976

Inverse kinematics

Physical equations

Pitch is a simple pendulum with the influence of gravity, and yaw is a simple pendulum without the influence of gravity.

State-space model



LQR Controller Design

Using feedforward to counteract gravity


The physics model changed into linear second-order system:


New state space model:


To standard form:


Design control law


Selecting the matrix Q and R


Solving the Riccati equation


Get matrix K


Final

Matlab code

output:

Angles:
psi_gamma (Yaw Angle): -0.54 rad
theta_gamma (Pitch Angle): -0.71 rad
theta_beta (Roll Angle): 0.37 rad
A:
         0         0         0    1.0000         0         0
         0         0         0         0    1.0000         0
         0         0         0         0         0    1.0000
         0         0         0   -0.4015         0         0
         0         0         0         0   -1.0153         0
         0         0         0         0         0         0

B:
   1.0e+03 *

         0         0         0
         0         0         0
         0         0         0
    0.4015         0         0
         0    1.0153         0
         0         0    0.3800

Q:
    2.0000         0         0         0         0         0
         0    2.0000         0         0         0         0
         0         0         0         0         0         0
         0         0         0    0.1000         0         0
         0         0         0         0    0.0100         0
         0         0         0         0         0         0

R:
     2     0     0
     0     5     0
     0     0     1

Inertia Matrix:
    0.0026   -0.0000    0.0000
   -0.0000    0.0025   -0.0000
    0.0000   -0.0000    0.0010

Head Position:
         0         0    0.0526

Head Joint Position:
     0     0     0

LQR Gain Matrix K:
    1.0000   -0.0000    0.0000    0.2335   -0.0000   -0.0000
   -0.0000    0.6325    0.0000   -0.0000    0.0560   -0.0000
    0.0000   -0.0000    0.0000   -0.0000   -0.0000    0.0000

X:
   -0.7093
   -0.5389
    0.3714
         0
         0
         0

Control Output u(t):
u(t) = -Kx + [mgl*sin(theta_gamma); 0; 0]
u(t) = [0.51; 0.34; -0.00]

code:

% Head Control Simulation with LQR
clc;
clear;

% Define angles of the body and head (in radians)
psi_beta = -0.1;   % Yaw angle of the body
theta_beta = pi/4; % Pitch angle of the body
phi_beta = 0;   % Roll angle of the body

theta_yaw = 0.5;  % Yaw angle of the head
theta_pitch = 0; % Pitch angle of the head
theta_roll = 0;  % Roll angle of the head

% Calculate the rotation matrix C_beta^alpha of the body
C_beta_alpha = [
    cos(theta_beta) * cos(psi_beta), cos(theta_beta) * sin(psi_beta), -sin(theta_beta);
    sin(phi_beta) * sin(theta_beta) * cos(psi_beta) - cos(phi_beta) * sin(psi_beta), ...
    sin(phi_beta) * sin(theta_beta) * sin(psi_beta) + cos(phi_beta) * cos(psi_beta), ...
    sin(phi_beta) * cos(theta_beta);
    cos(phi_beta) * sin(theta_beta) * cos(psi_beta) + sin(phi_beta) * sin(psi_beta), ...
    cos(phi_beta) * sin(theta_beta) * sin(psi_beta) - sin(phi_beta) * cos(psi_beta), ...
    cos(phi_beta) * cos(theta_beta)
];

% Calculate the rotation matrix C_gamma^beta of the head
C_gamma_beta = [
    cos(theta_pitch) * cos(theta_yaw), cos(theta_pitch) * sin(theta_yaw), -sin(theta_pitch);
    sin(theta_roll) * sin(theta_pitch) * cos(theta_yaw) - cos(theta_roll) * sin(theta_yaw), ...
    sin(theta_roll) * sin(theta_pitch) * sin(theta_yaw) + cos(theta_roll) * cos(theta_yaw), ...
    sin(theta_roll) * cos(theta_pitch);
    cos(theta_roll) * sin(theta_pitch) * cos(theta_yaw) + sin(theta_roll) * sin(theta_yaw), ...
    cos(theta_roll) * sin(theta_pitch) * sin(theta_yaw) - sin(theta_roll) * cos(theta_yaw), ...
    cos(theta_roll) * cos(theta_pitch)
];

% Calculate the overall rotation matrix C_gamma^alpha
C_gamma_alpha = C_beta_alpha * C_gamma_beta;

% Extract elements from the rotation matrix
r_11 = C_gamma_alpha(1, 1);
r_12 = C_gamma_alpha(1, 2);
r_13 = C_gamma_alpha(1, 3);
r_21 = C_gamma_alpha(2, 1);
r_22 = C_gamma_alpha(2, 2);
r_23 = C_gamma_alpha(2, 3);
r_31 = C_gamma_alpha(3, 1);
r_32 = C_gamma_alpha(3, 2);
r_33 = C_gamma_alpha(3, 3);

% Calculate the angles
psi_gamma = atan2(r_21, r_11); % Yaw angle
theta_gamma = atan2(-r_31, sqrt(r_32^2 + r_33^2)); % Pitch angle
theta_beta_calculated = atan2(r_32, r_33); % Roll angle

% Display results
fprintf('Angles:\n');
fprintf('psi_gamma (Yaw Angle): %.2f rad\n', psi_gamma);
fprintf('theta_gamma (Pitch Angle): %.2f rad\n', theta_gamma);
fprintf('theta_beta (Roll Angle): %.2f rad\n', theta_beta_calculated);

% Parameters
m = 0.60533; % Mass of the head (unknown data)
I = [0.0026313, -8.78814e-06, 2.99579e-05; 
     -8.78814e-06, 0.00249037, -5.33702e-06; 
     2.99579e-05, -5.33702e-06,  0.000984976]; % Inertia matrix
b = 0.001; % Damping coefficient
g = 9.81; % Acceleration due to gravity
l = 0.05258; % Length (assuming this is the distance from the pivot to the center of mass)

% Head position and joint position
head_position = [0, 0, 0.05258]; % Position of the head in space
head_joint_position = [0.0, 0.0, 0.0]; % Joint position of the head

% State-space representation
% Define the state vector: x = [theta_pitch; theta_yaw; theta_roll; dot_theta_pitch; dot_theta_yaw; dot_theta_roll]
x = zeros(6, 1); % Initial state (unknown data)
x(1) = theta_gamma; % Initial pitch angle
x(2) = psi_gamma;   % Initial yaw angle
x(3) = theta_beta_calculated; % Initial roll angle

% Define matrices A and B
I_pitch = I(2, 2); % Pitch inertia
I_yaw = I(3, 3);   % Yaw inertia
I_roll = I(1, 1);  % Roll inertia

% State matrix A
A = [0, 0, 0, 1, 0, 0;
     0, 0, 0, 0, 1, 0;
     0, 0, 0, 0, 0, 1;
     0, 0, 0, -b/I_pitch, 0, 0;
     0, 0, 0, 0, -b/I_yaw, 0;
     0, 0, 0, 0, 0, 0];

% Input matrix B
B = [0, 0, 0;
     0, 0, 0;
     0, 0, 0;
     1/I_pitch, 0, 0;
     0, 1/I_yaw, 0;
     0, 0, 1/I_roll];

% Define matrices Q and R for LQR
Q = diag([2, 2, 0, 0.1, 0.01, 0]); % State weighting matrix
R = diag([2, 5, 1]); % Control weighting matrix

% Display matrices
disp('A:');
disp(A);
disp('B:');
disp(B);
disp('Q:');
disp(Q);
disp('R:');
disp(R);

% Calculate the LQR controller gain
K = lqr(A, B, Q, R);

% Display results
disp('Inertia Matrix:');
disp(I);
disp('Head Position:');
disp(head_position);
disp('Head Joint Position:');
disp(head_joint_position);
disp('LQR Gain Matrix K:');
disp(K);
disp('X:');
disp(x);

% Calculate the total output
u_ff = [m * g * l * sin(theta_gamma); 0; 0]; % Feedforward term
u = -K * x + u_ff; % Control input
fprintf('Control Output u(t):\n');
fprintf('u(t) = -Kx + [mgl*sin(theta_gamma); 0; 0]\n');
fprintf('u(t) = [%.2f; %.2f; %.2f]\n', u(1), u(2), u(3));