Category Archives: Theory

Pressure sensor, absolute, differential and gauge ?

What’s the difference between absolute, differential and gauge pressure sensor ?


  • Absolute pressure sensor measures a pressure zero-referenced against a perfect vacuum, equal to gauge pressure plus atmospheric pressure.

  • Differential pressure sensor measures the difference of pressure between two points.

  • Gauge pressure sensor measure a pressure zero-referenced against ambient air pressure, so it is equal to absolute pressure minus atmospheric pressure.

Linear regression

The aim of this post is to explain how a linear regression is calculated with a very simple example. Assume we want to approximate a cloud of point with the following line : y=ax as illustrated bellow :

linear_regression

We want to minimize the error given by the square of the difference between points and the line y=ax:

 E=(y_1-a.x_1)^2+(y_2-a.x_2)^2+...+(y_n-a.x_n)^2

By expending each term, the equation becomes :

 E=y_1^2+a^2.x_1^2-2a.y_1.x_1 + y_2^2+a^2.x_2^2-2a.y_2.x_2 + ... + y_n^2+a^2.x_n^2-2a.y_n.x_n
 E=y_1^2 + y_2^2 + ... + y_n^2 + a^2(x_1^2 +x_2^2 +...+x_n^2) - 2a(y_1.x_1 + y_2.x_2 +y_n.x_n)

As we want to minimize the error, the value of a we are looking for is necessary a zero of the derivative of the error. Let’s calculate the derivative :

 \frac{\partial E}{\partial a} = 2a(x_1^2 +x_2^2 +...+x_n^2) - 2(y_1.x_1 + y_2.x_2 +y_n.x_n)

The error is minimized when  \frac{\partial E}{\partial a} =0 :

 2a(x_1^2 +x_2^2 +...+x_n^2) - 2(y_1.x_1 + y_2.x_2 +y_n.x_n)=0

It is now easy to calculate a :

 a=\frac{2(y_1.x_1 + y_2.x_2 +y_n.x_n)}{ 2(x_1^2 +x_2^2 +...+x_n^2) }
 a=\frac{ y_1.x_1 + y_2.x_2 +y_n.x_n}{ x_1^2 +x_2^2 +...+x_n^2 }
 a=\frac { \sum\limits_{i=1}^n x_i.y_i } {\sum\limits_{i=1}^n x_i^2}

Download

Quaternion conjugate

The quaternion conjugate is denoted by ^*. Considere the quaternion Q defined by:

 Q = \left[ \begin{matrix} a && b && c && d \end{matrix} \right]

The conjugate of Q is defined by:

 Q^* = \left[ \begin{matrix} a && -b && -c && -d \end{matrix} \right]

The conjugate can be used to swap the relative frames described by an orientation. For example, if {}^BQ_A describes the orientation of frame B relative to frame A, the conjugate of {}^BQ_A^*={}^AQ_B and describes the orientation of frame A relative to frame B.

Quaternions and rotations

A quaternion is a four-dimensional complex number that can be used to represent the orientation of a ridged body or coordinate frame in three-dimensional space. The general definition of a quaternion is given by:

 Q=a+b.i+c.j+d.k = \left[ \begin{matrix}a && b && c  && d\end{matrix}\right]

Representation

Let’s consider a vector \vec{V} defined by 3 scalars (V_x, V_y and V_z) and \theta an angle of rotation around \vec{V} :

quaternions

The quaternion associated to this transformation is given by:

 Q = \left[\begin{matrix}cos \frac{\theta}{2} && -V_x.sin \frac{\theta}{2} &&-V_y.sin \frac{\theta}{2} &&-V_z.sin \frac{\theta}{2} \end{matrix}\right]

Rotation around axes


Based on the previous formula, we can now calculate the quaternion defining a rotation around each axis:

Rotation around X

 Q_X=\left[\begin{matrix}cos \frac{\theta}{2} && -sin \frac{\theta}{2} &&0 &&0\end{matrix}\right]

Rotation around Y

 Q_Y=\left[\begin{matrix}cos \frac{\theta}{2} && 0 &&-sin \frac{\theta}{2} &&0\end{matrix}\right]

Rotation around Z

 Q_Z=\left[\begin{matrix}cos \frac{\theta}{2} && 0 &&0 &&-sin \frac{\theta}{2} \end{matrix}\right]

General rotation

A three dimensional vector (or point) can be rotated by a quaternion using the following relationship based on the quaternion product and quaternion conjugate:

 V_B = {}^BQ_A \otimes V_A \otimes {}^BQ_A^*

V_A and V_B are the quaternions associated to vectors \vec{v_A} and \vec{v_B}. Each quaternion contains a 0 inserted as the first element (rotation angle) to make them 4 element row quaternions, the other elements are given by the vector coordinates.

\vec{v_A} and \vec{v_B} is the same vector described in frame A and frame B respectively.

Example

This simple example shows the rotation of an ellipse defined in the plane {\vec{X},\vec{Y}}:

elipse_rotation

This example is based on the Madgwick’s quaternion library for Matlab:

Extended Kalman filter (EKF)

This post details the Kalman filter equations.

Predict

State prediction:

 \hat{x}_{i}^p = f(\hat{x}_{i-1},u_i)

Where:


  • \hat{x}_i^p is the predicted state at time step i.
  • \hat{x}_{i} is the estimate of state at time step i.
  • f is differential function that describes how the state will change according to the previous state (prediction) and the system input (u_i).
  • u_i is the system input at time step i.

Uncertainty (or covariance) prediction:

 P_i^p = F_i.P_{i-1}.F_i^\top + Q_i

Where:


  • P_i^p is the error covariance matrix predicted at time step i.
  • P_i is the estimated error covariance matrix associated with the estimated state \hat{x}_{i}.
  • Q_i is the system noise covariance matrix.
  • F_i is the transition matrix. It is given by the Jacobian of f()=\frac{df}{dx}

Update

Innovation or measurement residual:

 \widetilde{y}_i = z_i - h(\hat{x}_i^p)

Where:


  • \widetilde{y}_i is a measurement error : this is the difference between the measurement z_i and the estimate measurement from state \hat{x}_i^p.
  • z_i is an observation (or measurement) from the true state x_i.
  • h is a differential function which maps the state space into the observed space.

Innovation (or residual) covariance:

 S_i=H_i.P_i^p.H_i^\top+R_i

Where:


  • S_i is the covariance matrix associated to the measurement error \widetilde{y}_i.
  • R_i is the covariance matrix for the measurement noise.
  • H_i is a transition matrix which maps the state space into the observed space. It is given by H_i=\frac{dh}{dx}

Near-optimal Kalman gain:

 K_i=P_i^p.H_i^\top.S_i^{-1}

Where


  • K_i is the Kalman gain, this matrix contains the balance between prediction and observations. This matrix will weight the merging between predicted state and observations.

Updated state estimate:

 \hat{x}_i=\hat{x}_i^p + K_i.\widetilde{y}_i

Updated estimate covariance

 P_i=(I-K_i.H_i).P_i^p

Where:


  • I is the identity matrix.

Kalman filter

This post details the Kalman filter equations.

Predict

State prediction:

 \hat{x}_{i}^p = F_i.\hat{x}_{i-1} + B_i.u_i

Where:


  • \hat{x}_i^p is the predicted state at time step i.
  • \hat{x}_{i} is the estimate of state at time step i.
  • F_i is the transition matrix. It describes how the state will change according to the previous state (prediction).
  • B_i is a matrix that translates control input at time step i into a predicted change in state. In another words, it maps an input vector u_i into the state space.
  • u_i is the system input at time step i.

Uncertainty (or covariance) prediction:

 P_i^p = F_i.P_{i-1}.F_i^\top + Q_i

Where:


  • P_i^p is the error covariance matrix predicted at time step i.
  • P_i is the estimated error covariance matrix associated with the estimated state \hat{x}_{i}.
  • Q_i is the system noise covariance matrix.

Update

Innovation or measurement residual:

 \widetilde{y}_i = z_i - H_i.\hat{x}_i^p

Where:


  • \widetilde{y}_i is a measurement error : this is the difference between the measurement z_i and the estimate measurement from state \hat{x}_i^p.
  • z_i is an observation (or measurement) from the true state x_i.
  • H_i is a transition matrix which maps the state space into the observed space.

Innovation (or residual) covariance:

 S_i=H_i.P_i^p.H_i^\top+R_i

Where:


  • S_i is the covariance matrix associated to the measurement error \widetilde{y}_i.
  • R_i is the covariance matrix for the measurement noise.

Optimal Kalman gain

 K_i=P_i^p.H_i^\top.S_i^{-1}

Where


  • K_i is the Kalman gain, this matrix contains the balance between prediction and observations. This matrix will weight the merging between predicted state and observations.

Updated state estimate:

 \hat{x}_i=\hat{x}_i^p + K_i.\widetilde{y}_i

Updated estimate covariance

 P_i=(I-K_i.H_i).P_i^p

Where:


  • I is the identity matrix.

Kalman filter example

The Kalman filter is a very useful mathematical tool for merging multi-sensor data. We’ll consider a very simple example for understanding how the filter works. Let’s consider a robot that move in a single direction in front of a wall. Assume that the robot is equipped with two sensors : a speed measurement sensor and a distance measurement sensor (range finder). We’ll consider in the following that both sensors are noisy.

Robot

Our goal is to estimate, as accurately as possible, the position x of the robot:

position

Input of the system are a noisy distance measurement and a noisy velocity measurement:

rangefinder

Velocity

Result

result

Source code

close all;
clear all;
clc;

% Step time
dt=0.1;
% Variance of the range finder
SensorStd=0.08;
% Variance of the speed measurement
SpeedStd=0.5;

%% Compute the robot trajectory and simulate sensors
% Time
t=[0:dt:10];
n=size(t,2)
% Trajectory
x=0.05*t.*cos(2*t);
% Accelerometer
u=[0,diff(x)/dt] + SpeedStd*randn(1,n);
% Range finder
z=x-SensorStd*randn(1,n);



% Display position
figure;
plot (t,x);
grid on;
xlabel ('Time [s]');
ylabel ('Position [m]');
title ('Robot real position versus time x=f(t)');

% Display accelation
figure;
plot (t,u,'m');
grid on;
hold on;
plot (t,[0,diff(x)/dt]+3*SpeedStd,'k-.');
plot (t,[0,diff(x)/dt]-3*SpeedStd,'k-.');
xlabel ('Time [s]');
ylabel ('Velocity [m/s]');
title ('Velocity versus time u=f(t)');

% Display range finder
figure;
plot (t,z,'r');
grid on;
hold on;
plot (t,x+3*SensorStd,'k-.');
plot (t,x-3*SensorStd,'k-.');
xlabel ('Time [s]');
ylabel ('Distance [m]');
title ('Distance versus time z=f(t)');


%% Kalman

B=dt;
F=1;
Q=SpeedStd^2;
H=1;
R=SensorStd^2;

x_hat=[0];
y_hat=[0];
P=[0];
S=[0];

for i=2:n
        
    
    %% Prediction
    
    %State
    x_hat(i)=F*x_hat(i-1) + B*u(i-1);
    
    % State uncertainty
    P(i)=F*P(i-1)*F' + Q;
    
    
    
    %% Update    
    
    % Innovation or measurement residual
    y_hat(i)=z(i)-H*x_hat(i);
    
    % Innovation (or residual) covariance
    S=H*P(i)*H'+R;
    
    K=P(i)*H'*S;
    
    x_hat(i)=x_hat(i)+K*y_hat(i);
    
    P(i)=(1-K*H)*P(i);    
end

figure;
plot (t,x,'k');
hold on;
plot (t,x_hat);
plot (t,x_hat+sqrt(P),'r-.');
plot (t,x_hat-sqrt(P),'r-.');
legend('Real position','Estimated position','Uncertainty');

grid on;
xlabel ('Time [s]');
ylabel ('Distance [m]');
title ('Estimated position (speed only)');

Download

This example has been inspired by the very good tutorial of Bradley Hiebert-Treuer “An Introduction to Robot SLAM (Simultaneous Localization And Mapping)

Normal distribution

The normal (or Gaussian) distribution is a stochastic model commonly used for estimating sensor uncertainty. The law is given by:

 y(x)=\frac{1}{\sigma.\sqrt{2.\pi}} e^ {-\frac{(x-c)^2}{2\sigma^2}  }

Where:


  • \sigma is the standard deviation (square root of the variance)
  • c is the center of the gaussian

variance=\sigma^2

Here are some examples of normal distributions:
Examples

As the sum of probabilities must be equal to one thus the following surface is equal to one:

Surface

68, 95 and 99.7% of the surface is included in respectively \sigma, 2.\sigma and 3.\sigma:
distribution

Singular Value Decomposition of a 2×2 matrix

The following details how to compute the singular value decomposition (SVD) of a 2×2 matrix. For reminder :

 A=\left[ \begin{matrix} a & b \\ c & d \end{matrix} \right]=U  \cdot \Sigma \cdot V^T

where U and V are orthogonal and \Sigma is a diagonal matrix containing the singular values. In the particular case of a 2×2 matrix, the decomposition is given by:

 A=\left[ \begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \end{matrix} \right]  \cdot \left[ \begin{matrix} \sigma_1 & 0 \\ 0 & \sigma_2 \end{matrix} \right]  \cdot \left[ \begin{matrix} cos(\phi) & -sin(\phi) \\ sin(\phi) & cos(\phi) \end{matrix} \right]

With:

 \begin{matrix}V=C  \cdot  W=\left[ \begin{matrix} \pm{1} & 0 \\ 0 & \pm{1} \end{matrix} \right]\end{matrix}

Computation of U

\theta is given by:

 \theta=\frac{1}{2} atan2 (2ac+2bd , a^2+b^2-c^2-d^2)

The matrix U is given by:

 U=\left[ \begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \end{matrix} \right]

Computation of \Sigma

The singular values are given by:

 \begin{matrix} \sigma_1=\sqrt{ \frac {S_1 + S_2}{2} } \\\sigma_2=\sqrt{ \frac {S_1 - S_2}{2} } \\\end{matrix}

With:

 \begin{matrix} S_1 &=& a^2+b^2+c^2+d^2 \\S_2 &=& \sqrt{(a^2+b^2-c^2-d^2)^2 + 4(ac+bd)^2 } \end{matrix}

The matrix \Sigma is given by:

 \Sigma=\left[ \begin{matrix} \sigma_1 & 0 \\ 0 & \sigma_2 \end{matrix} \right]

Computation of V

First, let’s compute the temporary angle \Phi with the following formula:

 \Phi=\frac{1}{2} atan2 (2ab+2cd , a^2-b^2+c^2-d^2)

Let’s now compute the following scalars:

\begin{matrix}s_{11} &=& (a.c_\theta+c.s_\theta).c_\Phi &+& (b.c_\theta+d.s_\theta).s_\Phi \\s_{22} &=& (a.s_\theta-c.c_\theta).s_\Phi &+& (-b.s_\theta+d.c_\theta).c_\Phi \\\end{matrix}

where:

\begin{matrix}c_\Phi=cos(\Phi) \\s_\Phi=sin(\Phi) \\c_\theta=cos(\theta) \\ s_\theta=sin(\theta)\end{matrix}

The matrix V is given by:

 V=\left[ \begin{matrix} \frac {s_{11}} {|s_{11}|}cos(\Phi) & -\frac {s_{22}} {|s_{22}|}sin(\Phi) \\ \frac {s_{11}} {|s_{11}|} sin(\Phi) & \frac {s_{22}} {|s_{22}|}cos(\Phi) \end{matrix} \right]

Note that \frac {s_{11}} {|s_{11}|} and \frac {s_{22}} {|s_{22}|} are respectively the sign of s_{11} and s_{22}. If s_{11} or s_{22} are 0, dividing them by their absolute value will fail in most programming languages. The Matlab sign function returns 0 in this case (Thank you Corbie !).

Matlab function

%% Matlab function for computing the VSD of a 2x2 matrix
%% Written by Randy Ellis
%% More information at : https://www.lucidarme.me


function [U,SIG,V] = svd2x2(A)
% [U,SIG,V] = svd2x2(A) finds the SVD of 2x2 matrix A
% where U and V are orthogonal, SIG is diagonal,
% and A=U*SIG*V’
% Find U such that U*A*A’*U’=diag
Su = A*A';
phi = 0.5*atan2(Su(1,2)+Su(2,1), Su(1,1)-Su(2,2));
Cphi = cos(phi);
Sphi = sin(phi);
U = [Cphi -Sphi ; Sphi Cphi];


% Find W such that W’*A’*A*W=diag
Sw = A'*A;
theta = 0.5*atan2(Sw(1,2)+Sw(2,1), Sw(1,1)-Sw(2,2));
Ctheta = cos(theta);
Stheta = sin(theta);
W = [Ctheta, -Stheta ; Stheta, Ctheta];


% Find the singular values from U
SUsum= Su(1,1)+Su(2,2);
SUdif= sqrt((Su(1,1)-Su(2,2))^2 + 4*Su(1,2)*Su(2,1));
svals= [sqrt((SUsum+SUdif)/2) sqrt((SUsum-SUdif)/2)];
SIG =diag(svals);


% Find the correction matrix for the right side
S = U'*A*W
C = diag([sign(S(1,1)) sign(S(2,2))]);
V = W*C;

Download

This article has been inspired by the very good report of Randy Ellis. You can download this report with the following link:

Finding the transformation between two set of points

The aim of this article is to minimize the error between two clouds of points (minimization of the point to point error). Let’s considere the following cloud of points A in blue and B in red. Our goal is to find the transformation of B (translation and rotation) in order to minimize the distance point to point (according to the least square minimization).

figure0

 A=\left[ \begin{matrix} p_1^a & p_2^a & ... & p_n^a \end{matrix} \right]=\left[ \begin{matrix} x_1^a & x_2^a & ... & x_n^a \\ y_1^a & y_2^a & ... & y_n^a \end{matrix} \right]
 B=\left[ \begin{matrix} p_1^b & p_2^b & ... & p_n^b \end{matrix} \right]=\left[ \begin{matrix} x_1^b & x_2^b & ... & x_n^b \\ y_1^b & y_2^b & ... & y_n^b \end{matrix} \right]

We assume that the point matching has already being performed: each point of A is associated with a point of B.

figure1

Translation

First, compute the center of gravity of each cloud:
figure2

 \begin{matrix}COG^a=\frac{1}{n}.  \sum\limits_{i=1}^n p_i^a & COG^b=\frac{1}{n}.  \sum\limits_{i=1}^n p_i^b \end{matrix}

Translate each point with a translation vector given by the center of gravity fo its cloud (center each cloud on the origin).

 \begin{matrix} A

figure3

Rotation

For the rotation, it is a little bit more tricky. First calculate the following matrix:

 N= \sum\limits_{i=1}^n {p

Compute the singular value decomposition of N:

 N=U.\Sigma.V^T

The rotation matrix is given by:

 R=V.U^T

The rotation angle can now be extracted from the matrix R:

 \alpha=atan2(R_{21},R_{11})

By applying the rotation on the translated clouds, we get the following result:

figure4

Download

This article has been inspired by the very good bachelor thesis of Hans Martin Kjer and Jakob Wilm:

Least square approximation with a second degree polynomial

Let’s assume we want to approximate a point cloud with a second degree polynomial : y(x)=ax^2+bx+c.

SecondDegreePolynomial

The point cloud is given by n points with coordinates {x_i,y_i}. The aim is to estimate \hat{a}, \hat{b} and \hat{c} where y(x)=\hat{a}x^2+\hat{b}x + \hat{c} will fit the point cloud as mush as possible. We want to minimize for each point x_i the difference between y_i and y(x_i), ie. we want to minimize \sum\limits_{i=1}^n{(y_i-y(x_i))^2}. The matrix form of the system is given by:

 \left[ \begin{matrix} {x_1}^2 & x_1 & 1 \\{x_2}^2 & x_2 & 1 \\... & ... & ... \\{x_n}^2 & x_n & 1 \\\end{matrix} \right].\left[ \begin{matrix} \hat{a} \\\hat{b} \\\hat{c} \end{matrix} \right]=\left[ \begin{matrix} y_1 \\y_2 \\... \\y_n \\\end{matrix} \right]

Let’s define A, B and \hat{x}:

\begin{matrix}  A=\left[ \begin{matrix} {x_1}^2 & x_1 & 1 \\{x_2}^2 & x_2 & 1 \\... & ... & ... \\{x_n}^2 & x_n & 1 \\\end{matrix} \right] & B=\left[ \begin{matrix} y_1 \\y_2 \\...  \\y_n \\\end{matrix} \right]&\hat{x}=\left[ \begin{matrix} \hat{a} \\ \hat{b} \\ \hat{c} \end{matrix} \right] \end{matrix}

The system is now given by:

 A.\hat{x}=B

The optimal solution is given by:

 \hat{x}=A^{+}.B = A^{T}(A.A^{T})^{-1}.B

Where A^{+} is the pseudoinverse of A given by A^{+}=A^{T}(A.A^{T})^{-1}.

Donwload

Least square linear approximation

This post explains how to make linear approximation with a simple example. Let’s assume we want to approximate a point cloud with a linear function : y(x)=ax+b.

LinearApproximation

The point cloud is given by n points with coordinates {x_i,y_i}. The aim is to estimate \hat{a} and \hat{b} where y(x)=\hat{a}x+\hat{b} will fit the point cloud as mush as possible. We want to minimize for each point x_i the difference between y_i and y(x_i), ie. we want to minimize \sum\limits_{i=1}^n{(y_i-y(x_i))^2}. The matrix form of the system is given by:

 \left[ \begin{matrix} x_1 & 1 \\x_2 & 1 \\... & ... \\x_n & 1 \\\end{matrix} \right].\left[ \begin{matrix} \hat{a} \\\hat{b}\end{matrix} \right]=\left[ \begin{matrix} y_1 \\y_2 \\... \\y_n \\\end{matrix} \right]

Let’s define A, B and \hat{x}:

 \begin{matrix}A=\left[ \begin{matrix} x_1 & 1 \\ x_2 & 1 \\ ... & ... \\ x_n & 1 \\ \end{matrix} \right] &B=\left[ \begin{matrix} y_1 \\y_2 \\...  \\y_n \\\end{matrix} \right] &\hat{x}=\left[ \begin{matrix} \hat{a} \\\hat{b}\end{matrix} \right] \end{matrix}

The system is now given by:

 A.\hat{x}=B

The optimal solution is given by:

 \hat{x}=A^{+}.B = A^{T}(A.A^{T})^{-1}.B

Where A^{+} is the pseudoinverse of A given by A^{+}=A^{T}(A.A^{T})^{-1}.

Donwload

Ball joint connecting link

The aim of this post deals with ball joint connecting link. The following model is considered:

Drawing

Kinematic

This part explains how to calculate \Theta_2=f(\Theta_1).

DrawingKinematic

The angle \Theta_2 will be calculated with the following relation:

 \Theta_2=\pi- \widehat{P_2MP_1} -\widehat{P_1MO}

First, let’s calculate the coordinates of P_1:

 P_1=\begin{pmatrix} x_1 \\ y_1 \end{pmatrix} = \begin{pmatrix} l_1.cos(\Theta_1) \\ l_1.sin(\Theta_1) \end{pmatrix}

Based on the previous result, let’s calculate the distance between P_1 and M:

 \| P_1M \| = \sqrt{ (x_1-X)^2 + y_1^2 }

In the triangle HP_1M, rectangle in H, it becomes easy to calculate the angle \widehat{P_1MO}:

 \widehat{P_1MO}=atan2(\|HP_1\|,\|MH\|)=atan2(y_1,X-x_1)

Now we can calculate the angle \widehat{P_2MP_1} in the triangle P_1MP_2 thanks to the Al Kashi’s theorem:

 \|P_1P_2\| = \|MP_1\|^2 + \|MP_2\|^2 -2.\|MP_1\|.\|MP_2\|.cos(\widehat{P_2MP_1})

 L = \|MP_1\|^2 + {l_2}^2 -2.\|MP_1\|.l_2.cos(\widehat{P_2MP_1})

and finally:

 cos(\widehat{P_2MP_1}) = \frac{\|MP_1\|^2 + {l_2}^2 - L} {2.\|MP_1\|.l_2}

In conclusion:

 \Theta_2=\pi \pm acos\left(\frac{ (x_1-X)^2 + y_1^2  + {l_2}^2 - L} {2.l_2.\sqrt{ (x_1-X)^2 + y_1^2 }} \right) - atan2(y_1,X-x_1)

where:

 \begin{pmatrix} x_1 \\ y_1 \end{pmatrix} = \begin{pmatrix} l_1.cos(\Theta_1) \\ l_1.sin(\Theta_1) \end{pmatrix}

Torque

This second part explains how to estimate the torque transmitted. Let’s assume that the actuated rod is the left one (centered on O). The torque provided on this rod is T_1. The aim is to calculate the torque transmitted (T_2) on the second rod (centered on M). The purpose of this second part is to express T_2=g(T_1).

DrawingTorque

First, let’s calculate the coordinates of P_2:

 P_2=\begin{pmatrix} x_2 \\ y_2 \end{pmatrix} = \begin{pmatrix} X+l_2.cos(\Theta_1) \\ l_2.sin(\Theta_1) \end{pmatrix}

Let’s calculate the amplitude of the force \vec{F_1}:

 \| \vec{F_1}\| = \frac{T_1}{l_1}

Now, calculate the angle \alpha between \vec{F_1} and \vec{F_b}:

 \alpha = ( \widehat{ \vec{F_1} ; \vec{F_b} } )= ( \widehat{ \vec{OM} ; \vec{F_1} } ) - ( \widehat{ \vec{OM} ; \vec{F_2} } ) = ( \widehat{ \vec{OM} ; \vec{F_1} } ) -( \widehat{ \vec{OM} ; \vec{P_1P_2} } )

Let’s calulate the angles:

 ( \widehat{ \vec{OM} ; \vec{F_1} } ) = \Theta_1 - \frac{\pi}{2}

And :

 ( \widehat{ \vec{OM} ; \vec{P_1P_2} } ) = atan2(y_2-y_1,x_2-x_1)

Now \alpha can be calculated:

 \alpha = \Theta_1 - \frac{\pi}{2} - atan2(y_2-y_1,x_2-x_1)  = \Theta_1 - atan2(x_2-x_1,y_1-y_2)

and F_b can also now be calculated:

 \| \vec{F_b} \| = \| \vec{F_1} \|.cos(\alpha)

As it was done for \alpha, let’s calculate the angle \beta between \vec{F_b} and \vec{F_2}:

 \beta = ( \widehat{ \vec{F_2} ; \vec{F_b} } )= \Theta_2 - \frac{\pi}{2} - atan2(y_2-y_1,x_2-x_1)  =  \Theta_2 - atan2(x_2-x_1,y_1-y_2)

The force F_2 can now also be calculated:

 \| \vec{F_2} \| = \| \vec{F_b} \|.cos(\beta)

The outpout torque is :

 T_2=\| \vec{F_2} \|.l_2

In conclusion:

 T_2= \frac{l_2}{l_1}. T_1.cos(\beta).cos(\alpha)

where:

 \alpha = \Theta_1 - atan2(x_2-x_1,y_1-y_2)

and

 \beta =  \Theta_2 - atan2(x_2-x_1,y_1-y_2)

Download

Ball and beam : model

This post deals describe the model of the ball and beam. The system consists of a long beam which can be tilted by an actuator together with a ball rolling back and forth on top of the beam.

BallAndBeamModel

We will make the following assumptions:

  • m is the mass of the ball

  • \Theta is the angle of the beam

  • x is the position of the ball on the beam

  • b is the friction coefficient of the ball


Based on the fundamental principle of dynamics and considering that the friction is proportional to the speed, we can write:

 m.a = m.\ddot{x} = \sum{\vec{F}} = m.g.sin(\Theta) - b.\dot{x}

In conclusion, the differential equation of the system is:

 \ddot{x} = - \frac{b}{m}.\dot{x} + g.sin(\Theta)

Inverted pendulum : model without friction

Introduction

This post presents the model of an inverted pendulum. This device is composed of an activated trolley and a pendulum which has its center of mass above its pivot point.

InvertedPendulum

In this post, we will assume the following hypotheses:


  • m_1 is the mass of the trolley.

  • m_2 is the mass of the pendulum.

  • I_1 is the moment of inertia of the trolley.

  • I_2 is the moment of inertia of the pendulum.

  • L is the distance between the pivot and the center of mass of the pendulum.

  • \Theta is the angle of inclination of the pendulum.

  • x_1 and y_1 are the coordinates of the center of mass C_1 of the trolley .

  • \Theta_1 is the orientations of the trolley.

  • x_2 and y_2 are the coordinates of the center of mass C_2 of the pendulum.

  • \Theta_2 is the orientations of the pendulum.

The following notation will be used in this article to avoid too complex equations for the derivatives:

 \frac{d\Theta}{dt} = \dot{\Theta}

And for the second derivative:

 \frac{d^2\Theta}{dt^2} = \ddot{\Theta}

Constraints

As the trolley and the pendulum are linked by a pivot, the following equations can be written. This equations expresses the coordinate and orientation of each body of our system. For the trolley (body 1):

\begin{array}{r c l} x_1 &=& x_1 \\ y_1 &=& 0 \\ \Theta_1 &=& 0 \\\end{array}

For the pendulum (body 2):

\begin{array}{r c l} x_2 &=& x_1 - L.sin(\Theta)\\ y_2 &=& y_1 + L.cos(\Theta) &=& L.cos(\Theta)\\ \Theta_2 &=& \Theta\end{array}

Let’s define the vector \vec{q}, the position of our system :

 \vec{q}=\begin{pmatrix} x_1 \\ \Theta_2 \end{pmatrix}

Let’s define the constraints on position:

 \phi = \begin{pmatrix} y_1 \\ \Theta_1 \\ x_2 \\ y_2 \end{pmatrix} = \begin{pmatrix} 0 \\ 0 \\ x_1 - L.sin(\Theta) \\ L.cos(\Theta) \end{pmatrix}

Let’s calculate the constraints on velocity:

 \dot{\phi} = \begin{pmatrix} \dot{y_1} \\ \dot{\Theta_1} \\ \dot{x_2} \\ \dot{y_2} \end{pmatrix} = \begin{pmatrix} 0 \\ 0 \\  \dot{x_1} - L.\dot{\Theta_2}.cos(\Theta_2)  \\ -L.\dot{\Theta_2}.sin(\Theta_2) \end{pmatrix}

Let’s calculate the constraints on acceleration:

 \ddot{\phi} = \begin{pmatrix} \dot{y_1} \\ \ddot{\Theta_1} \\ \ddot{x_2} \\ \ddot{y_2} \end{pmatrix} = \begin{pmatrix} 0 \\ 0 \\  \ddot{x_1} - L.\ddot{\Theta_2}.cos(\Theta_2) + L.\dot{\Theta_2}^2.sin(\Theta_2) \\ -L.\ddot{\Theta_2}.sin(\Theta_2) - L.\dot{\Theta_2}^2.cos(\Theta_2) \end{pmatrix}

Dynamics

The fundamental principle of dynamics can be apply to our system. For the trolley (body 1):

\begin{array}{r c l} m_1.{a_1}^x = m_1.\ddot{x_1}&=& \| \vec{F_t} \| + \lambda_1 \\ m_1.{a_1}^y = m_1.\ddot{y_1}&=& 0 + \lambda_2 \\ I_1.\ddot{\Theta_1} &=& 0 + \lambda_3 \\\end{array}

For the pendulum (body 2):

\begin{array}{r c l} m_2.{a_2}^x = m_2.\ddot{x_2} &=& 0 + \lambda_4 \\  m_2.{a_2}^y = m_2.\ddot{y_2} &=& \| \vec{F_g} \| + \lambda_5 = -g.m_2 + \lambda_5\\ I_2.\ddot{\Theta_2} &=& 0 + \lambda_6 \\\end{array}

Where \lambda_1, \lambda_2,\lambda_3,\lambda_4,\lambda_5 and \lambda_6 are the Lagrange’s coefficients. These coefficients represent the interaction between bodies. In the present system, this is the interaction between the pendulum and the trolley.

Interactions

We will now solve the system in order to eliminate the interactions between bodies (\lambda_i). It is proven that:

 M.\ddot{q} = F - \frac{d\phi}{dq}^\top .\Lambda

Where :

 M=\begin{pmatrix} m_1 & 0 \\ 0 & I_2 \end{pmatrix}
 \ddot{q}=\begin{pmatrix} \ddot{x_1} \\ \ddot{\Theta_2} \end{pmatrix}
 F=\begin{pmatrix} \| \vec{F_t} \|  \\ 0 \end{pmatrix}
 \frac{d\phi}{dq} = \begin{pmatrix} \frac{dy_1}{dx1} & \frac{dy_1}{d\Theta_2} \\\frac{d\Theta_1}{dx1} & \frac{d\Theta_1}{d\Theta_2} \\\frac{dx_2}{dx1} & \frac{dx_2}{d\Theta_2} \\\frac{dy_2}{dx1} & \frac{dy_2}{d\Theta_2} \end{pmatrix} =\begin{pmatrix}0 & 0 \\0 & 0 \\1 & -L.cos(\Theta_2) \\0 & -L.sin(\Theta_2)\end{pmatrix}
 \Lambda=\begin{pmatrix} \lambda2 \\ \lambda3 \\ \lambda4 \\ \lambda5 \end{pmatrix}

Applied to our system, this gives:

\begin{pmatrix} m_1 & 0 \\ 0 & I_2 \end{pmatrix} .\begin{pmatrix} \ddot{x_1} \\ \ddot{\Theta_2} \end{pmatrix} =\begin{pmatrix} \| \vec{F_t} \|  \\ 0 \end{pmatrix} -\begin{pmatrix} 0 & 0 & 1 & 0 \\ 0 & 0 & -L.cos(\Theta_2) & -L.sin(\Theta_2) \end{pmatrix} .\begin{pmatrix} \lambda2 \\ \lambda3 \\ \lambda4 \\ \lambda5 \end{pmatrix}

and:

\begin{array}{r c l}m_1.\ddot{x_1} &=& \| \vec{F_t} \| -\lambda4 \\I_2.\ddot{\Theta_2} &=& \lambda4.L.cos(\Theta_2) + \lambda5.L.sin(\Theta_2)\end{array}

Solving the system

Based on the previous equations, we can formulate \lambda_4 and \lambda_5:

\begin{array}{r c l}\lambda_4 &=& m_2.\ddot{x_2} \\\lambda_5 &=& m_2.\ddot{y_2} + g.m_2 \\\end{array}

The previous equations become:

\begin{array}{r c l}m_1.\ddot{x_1} &=& \| \vec{F_t} \| -m_2.\ddot{x_2} \\I_2.\ddot{\Theta_2} &=& m_2.\ddot{x_2}.L.cos(\Theta_2) + (m_2.\ddot{y_2} + g.m_2).L.sin(\Theta_2)\end{array}

Thanks to the previous constraints on acceleration:

\begin{array}{r c l}m_1.\ddot{x_1} &=& \| \vec{F_t} \| -m_2.(\ddot{x_1} - L.\ddot{\Theta_2}.cos(\Theta_2) - L.\dot{\Theta_2}^2.sin(\Theta_2) ) \\I_2.\ddot{\Theta_2} &=& m_2.(\ddot{x_1} - L.\ddot{\Theta_2}.cos(\Theta_2) + L.\dot{\Theta_2}^2.sin(\Theta_2) ).L.cos(\Theta_2) + (m_2.(-L.\ddot{\Theta_2}.sin(\Theta_2) - L.\dot{\Theta_2}^2.cos(\Theta_2)) + g.m_2).L.sin(\Theta_2)\end{array}

Rewriting the previous equation gives us:

\begin{array}{r c l}(m_1+m_2).\ddot{x_1} &-& m_2.L.cos(\Theta_2).\ddot{\Theta_2} &=& \| \vec{F_t} \| - m_2.L.\dot{\Theta_2}^2.sin(\Theta_2) \\-m_2.L.cos(\Theta_2).\ddot{x_1} &+& (I_2+ m_2.L^2).\ddot{\Theta_2} &=& m_2.g.L.sin(\Theta_2)\end{array}

And:

\begin{pmatrix} m_1+m_2 & - m_2.L.cos(\Theta_2) \\-m_2.L.cos(\Theta_2) & (I_2+ m_2.L^2)\end{pmatrix} . \ddot{q} =\begin{pmatrix} \| \vec{F_t} \| - m_2.L.\dot{\Theta_2}^2.sin(\Theta_2) \\ m_2.g.L.sin(\Theta_2)\end{pmatrix}

The general equation of the system is:

\ddot{q}=A^{-1}.B

Where:

A=\begin{pmatrix} m_1+m_2 & - m_2.L.cos(\Theta_2) \\-m_2.L.cos(\Theta_2) & (I_2+ m_2.L^2)\end{pmatrix}
B= \begin{pmatrix} \| \vec{F_t} \| - m_2.L.\dot{\Theta_2}^2.sin(\Theta_2) \\ m_2.g.L.sin(\Theta_2)\end{pmatrix}

State space representation

Based on the previous result, it is now possible to write the state space model of the system. Let’s define X the state of the system:

X=\begin{pmatrix} x_1 \\ \Theta_2 \\ \dot{x_1} \\ \dot{\Theta_2} \end{pmatrix}

The state space representation is:

\dot{X}=\begin{pmatrix} \dot{x_1} \\ \dot{\Theta_2} \\ \left[A^{-1}.B\right] \end{pmatrix}

Acknowledgements

I want to thank Sébastien Lagrange from the University of Angers for his help and explanations.

Simple pendulum

Introduction


A simple pendulum is a weight suspended by a non-deformable wire.

PenduleumOverview

In this post, we will assume the following hypotheses:


  • The weight can swing freely.

  • L is the wire’s lenght.

  • m is the mass of the pendulum’s body.

  • The mass of the wire is neglected.

  • The frictions are neglected.

  • Equation of the system

    To find the equation of the system, we will apply the fundamental principle of dynamics:

     m.\vec{a} = \sum{\vec{F}}

    Forces acting on the system are the gravity force \vec{F_g} and the wire tension force \vec{T}. The body’s trajectory describes an arc of circle. The body’s acceleration is tangent to this trajectory. The wire tension force is perpendicular to the body’s trajectory. Its projection along the motion axis is null. Only one force have to be considered in the following : the gravity force:

     m.a = \vec{F_m} = m.g.sin(\Theta)

    The relation between angular and linear velocity is given by :

     v=L\frac{d \Theta }{dt}

    By derivating the previous equation, we get:

     a=\frac{dv}{dt}=L. \frac{d^2 \Theta }{dt^2}

    The equation calculated from the fundamental principle of dynamics can be updated:

     m.L. \frac{d^2 \Theta }{dt^2} = m.g.sin(\Theta)

    Dividing by m:

     L. \frac{d^2 \Theta }{dt^2} = g.sin(\Theta)

    The differential equation of the system is:

     \frac{d^2 \Theta }{dt^2} = \frac{g}{L}.sin(\Theta)

    Solving the equation

    The previous differential equation is non linear. A commonly used approximation is to approximate sin(\Theta) by \Theta for small values of \Theta. The equation becomes linear:

     \frac{d^2 \Theta }{dt^2} = \frac{g}{L}.\Theta

    The general solution to the previous differential equation is:

     \Theta = A.cos(\omega.t + \phi)

    With
     \omega^2=\frac{g}{L}

    Given the initial conditions \Theta=\Theta_0 (initial angle of the pendulum), and assuming that the initial speed is null:

    A.cos(\phi)=\Theta_0
    -A.\omega.sin(\phi)=0

    The previous system can be solved:

    A=\Theta_0
    \phi=0

    The general solution of the system is :

     \Theta(t)=\Theta_0.cos(\omega.t)

    Where \omega = \sqrt { \frac {g}{L} }. The period of the motion is given by T=2.\pi.\sqrt { \frac {g}{L}}.

    Acknowledgements

    I want to thank Jean-Pierre Pecqueur from the University of Angers for his help.