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));