Robot modelling, full course

1.1 What “locating” means

Locating an object in space is giving two pieces of information relative to a reference frame R₀ attached to the environment:

1. the position of a particular point of the object (centre of gravity, corner, tool centre…): 3 coordinates (x, y, z) in R₀;
2. the orientation of the object: 3 angles which characterise in a unique way the orientation of the frame R₁ linked to the object relative to R₀.

Result: 6 parameters (6 DOF) for a free solid.

1.2 The rotation matrix

Let R₁ = (O, x₁, y₁, z₁) be turned relative to R₀ = (O, x₀, y₀, z₀). The columns of the rotation matrix ⁰R₁ are the vectors of R₁ expressed in R₀ (direction cosines):

⁰R₁ = x₁·x₀y₁·x₀z₁·x₀x₁·y₀y₁·y₀z₁·y₀x₁·z₀y₁·z₀z₁·z₀ = r₁₁r₁₂r₁₃r₂₁r₂₂r₂₃r₃₁r₃₂r₃₃, ⁰OP = ⁰R₁ ¹OP

The left exponent tells in which frame the coordinates are written. ⁰R₁ gives the old coordinates (in R₀) as a function of the new ones (in R₁).

1.3 The three elementary rotations

Rot(x, θ) = 1000cos θ−sin θ0sin θcos θ
Rot(y, θ) = cos θ0sin θ010−sin θ0cos θ
Rot(z, θ) = cos θ−sin θ0sin θcos θ0001

Mnemonic: the row and the column of the rotation axis contain a 1 and zeros; the “−sin” is above the diagonal for x and z, but below it for y (circular order x → y → z → x: for y, we turn from z towards x).

Key terms

• reference frame
• rotation matrix
• direction cosines
• orthogonal matrix

A rotation matrix contains 9 numbers but only 3 are independent. We therefore describe the orientation with 3 angles, by chaining 3 elementary rotations. Two conventions are on the programme.

2.1 Euler angles (ZXZ convention): rotations around moving axes

We start from the frame A and perform successively:

1. a rotation of angle φ (precession) around z: x becomes u;
2. a rotation of angle θ (nutation) around the new axis x (= u): z becomes z′;
3. a rotation of angle ψ (proper rotation) around the new axis z′: we obtain the frame B.

The rotation axes move with the frame: we multiply on the right, in the order of the rotations.

^AR_B = R_Euler = Rot(z, φ) Rot(x, θ) Rot(z, ψ) = cφcψ − sφcθsψ−cφsψ − sφcθcψsφsθsφcψ + cφcθsψ−sφsψ + cφcθcψ−cφsθsθsψsθcψcθ

with cφ = cos φ, sφ = sin φ, etc. Usual domains: φ ∈ [0, 2π[, θ ∈ [0, π], ψ ∈ [0, 2π[.

2.2 Cardan angles / Roll–Pitch–Yaw (XYZ fixed convention)

This time we turn around the fixed axes of space (a rotation does not modify the direction of the next one). It is the convention of free vehicles in space: drones, boats, mobile robots. Vocabulary from navigation:

• x forward, z vertical towards the sky, y to the left (right-handed frame);
• roll γ around x, pitch β around y, yaw α around z.

The first rotation (around x) is done on the object; the following ones are “added” on the left:

^AR_B = R_Cardan = Rot(z, α) Rot(y, β) Rot(x, γ) = cαcβcαsβsγ − sαcγcαsβcγ + sαsγsαcβsαsβsγ + cαcγsαsβcγ − cαsγ−sβcβsγcβcγ

2.3 Inverse problem: finding the angles from the matrix

We know ^AR_B = (r_ij) and look for the Cardan angles (α, β, γ). We identify term by term.

Why atan2(y, x) and not arctan(y/x)? Because atan2 uses the signs of y and of x to give the angle in the right quadrant (]−π, π]), whereas arctan only returns an angle between −π/2 and π/2.

Key terms

• Euler angles
• Cardan / roll–pitch–yaw
• gimbal lock
• atan2

3.1 The idea: one tool for the rotation and the translation

In robotics, a frame R_j is both turned and moved relative to R_i. We group both pieces of information in a 4 × 4 matrix:

ⁱT_j = ( ⁱR_j | ⁱP_j ; 0 0 0 | 1 ) = s_xn_xa_xP_xs_yn_ya_yP_ys_zn_za_zP_z0001

• the 3×3 block is the rotation matrix (without rotation: the identity);
• the 4th column contains the coordinates of the origin O_j in R_i;
• the last row is always (0 0 0 1).

The columns s, n, a are the vectors x_j, y_j, z_j expressed in R_i (Khalil–Dombre notation).

3.2 Homogeneous coordinates of a point, homogeneous components of a vector

3.3 Elementary transformations

Trans(a, b, c) = 100a010b001c0001 Rot(x, θ) = 10000cos θ−sin θ00sin θcos θ00001

Applied to a point: Trans(a, b, c) (x, y, z, 1)^T = (x+a, y+b, z+c, 1)^T; applied to a vector, it leaves it unchanged.

3.4 Composition and inverse

⁰T_n = ⁰T₁ ¹T₂ ²T₃ … ⁿ⁻¹T_n ^jT_i = (ⁱT_j)⁻¹ = ( ⁱR_j^T −ⁱR_j^{T i}P_j ; 0 0 0 | 1 )

The global transformation of a 6-axis robot is therefore ⁰T₆ = ⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆: it allows to express in the base frame R₀ any point or vector known in the frame of the end effector R₆.

Key terms

• homogeneous matrix
• homogeneous coordinates
• composition
• inverse transformation

4.1 Why a convention?

There are infinitely many ways of placing a frame on each body. The DHm method (Khalil–Kleinfinger) imposes rules which allow describing a chain of n bodies with a minimal number of parameters: each passage R_j−1 → R_j only requires 4 parameters (instead of 6).

Notation: the body C_j carries the frame R_j; the joint L_j links C_j−1 to C_j; C₀ is the base.

4.2 The rules to place the frames

The common perpendicular between two lines:

• two arbitrary (non-coplanar) lines: it is unique;
• two concurrent lines: it is unique (normal to the plane, through the intersection point);
• two parallel lines: infinitely many solutions (parallel to each other, of variable height) ⇒ we choose the one that cancels a maximum of parameters;
• two coincident lines: double infinity of solutions (free height and orientation).

Conventions for the ends: we choose R₀ coincident with R₁ when q₁ = 0 (often z₀ vertical, O₀ at the base); for the last frame R_n, we choose x_n coincident with x_n−1 when q_n = 0, or on a symmetry axis of the end effector.

4.3 The 4 parameters to go from R_j−1 to R_j

(In the Master 2016 course, d_j is written a_j: it is the same parameter.)

4.4 The transition matrix ^j−1T_j

We chain 4 elementary movements (in this order):

^j−1T_j = Rot(x, α_j) Trans(x, d_j) Rot(z, θ_j) Trans(z, r_j)

^j−1T_j = cos θ_j−sin θ_j0d_jcos α_j sin θ_jcos α_j cos θ_j−sin α_j−r_j sin α_jsin α_j sin θ_jsin α_j cos θ_jcos α_jr_j cos α_j0001

Frequent cases to know by heart:

• α_j = 0 (parallel axes): ^j−1T_j = cθ_j−sθ_j0d_jsθ_jcθ_j00001r_j0001
• α_j = −π/2: the 2nd row becomes (0 0 1 r_j) and the 3rd (−sθ_j −cθ_j 0 0).
• α_j = +π/2: the 2nd row becomes (0 0 −1 −r_j) and the 3rd (sθ_j cθ_j 0 0).

4.5 A complete example: the SCARA robot (4 DOF, RRPR)

The SCARA has 4 moving bodies + the base, so 5 frames R₀ … R₄. Joints: pivot q₁, pivot q₂, slider q₃ (vertical), pivot q₄ (rotation of the tool). The four z axes are vertical and parallel.

4 joint variables (q₁, q₂, q₃, q₄) and 4 non-zero dimensional parameters (r₁, d₂, d₃, r₄). The sign −r₄ comes from O₄ being lower than O₃; q₃ is counted along z₃.

The 4 matrices:

⁰T₁ = c₁−s₁00s₁c₁00001r₁0001 ¹T₂ = c₂−s₂0d₂s₂c₂0000100001

²T₃ = 100d₃0100001q₃0001 ³T₄ = c₄−s₄00s₄c₄00001−r₄0001

Calculation trick: we multiply starting from the end, ²T₄ = ²T₃ ³T₄, then ¹T₄ = ¹T₂ ²T₄, then ⁰T₄ = ⁰T₁ ¹T₄. We write c₁₂ = cos(q₁+q₂), c₁₂₄ = cos(q₁+q₂+q₄), etc. Result:

⁰T₄ = c₁₂₄−s₁₂₄0d₂c₁ + d₃c₁₂s₁₂₄c₁₂₄0d₂s₁ + d₃s₁₂001r₁ + q₃ − r₄0001

Interpretation: the tool turns by an angle q₁ + q₂ + q₄ around the vertical, and its position is given by the 4th column.

4.6 6R robots: examples of tables

Kuka KR 160 R1570 (axis 2 shifted by 250 mm from axis 1; wrist with 3 concurrent axes):

r₁: height of the shoulder; d₂: offset axis 1 / axis 2; d₃: length of the arm; r₄: length of the forearm.

ABB IRB 140 / IRB 120 (no offset between axes 1 and 2: d₂ = 0 and O₀ = O₁ = O₂) – table of the Master course:

Key terms

• DHm convention
• common perpendicular
• joint variable
• transition matrix

Compare with the SCARA DHM table →

5.1 Definition

The MGD gives the situation of the end effector (position + orientation) as a function of the joint variables:

X = f(q) ⟺ ⁰T_n(q) = ⁰T₁(q₁) ¹T₂(q₂) … ⁿ⁻¹T_n(q_n)

• The position of the end effector is read in the 4th column: ⁰O_n = ⁰T_n (0, 0, 0, 1)^T.
• The orientation is read in the 3×3 block (then, if needed, we extract Euler or Cardan angles, step 2).
• For a tool offset ⁿE in R_n: ⁰E = ⁰T_n ⁿE, or ⁰T_E = ⁰T_n ⁿT_E.

The MGD always has a unique solution: to a configuration q corresponds one single situation of the tool.

5.2 MGD of the SCARA

With the notations of course C1 (a₁: length of arm 1, a₂: arm 2, h: height), we have a₁ = d₂, a₂ = d₃, h = r₁ − r₄ in DHm:

X_E = a₁ cos q₁ + a₂ cos(q₁+q₂)
Y_E = a₁ sin q₁ + a₂ sin(q₁+q₂)
Z_E = h + q₃
θ_E = q₁ + q₂ + q₄

It is exactly the 4th column and the rotation angle of ⁰T₄ found by DHm: the geometric method “by hand” and the DHm method give the same result. For a 6R robot, only the DHm method remains practicable.

Key terms

• MGD
• situation
• tool offset

See the MGD computed live on the arm →

6.1 Statement of the problem

The MGI gives the joint variables allowing to reach a wanted situation of the tool:

q = f⁻¹(X) that is: solve ⁰T_n(q) = U₀ = s_xn_xa_xP_xs_yn_ya_yP_ys_zn_za_zP_z0001

It is the model used permanently by the control cabinet: the programmer gives points in space (in X), the motors need set-points in q.

6.2 MGI of the SCARA, step by step

We know (X_E, Y_E, Z_E, θ_E) and look for (θ₁, θ₂, d₃, θ₄).

Step a – find θ₂. We square and add the first two equations of the MGD:

X_E² + Y_E² = a₁² + a₂² + 2a₁a₂ cos θ₂ ⟹ cos θ₂ = X_E² + Y_E² − (a₁² + a₂²)2a₁a₂

θ₂ = δ arccos( X_E² + Y_E² − (a₁² + a₂²)2a₁a₂ ), δ = ±1

The choice δ = ±1 is the choice of the aspect (elbow right or left). If the quantity in the arccos leaves [−1, 1], the point is unreachable.

Step b – find θ₁. The polar angle of E is atan2(Y_E, X_E); we must subtract the angle between arm 1 and the line OE, which is atan2(a₂ sin θ₂, a₁ + a₂ cos θ₂):

θ₁ = atan2(Y_E, X_E) − atan2(a₂ sin θ₂, a₁ + a₂ cos θ₂)

Step c – the translation and the orientation:

d₃ = Z_E − h θ₄ = θ_E − θ₁ − θ₂

6.3 General method: Paul's method (Khalil–Dombre)

For a 6-DOF robot, we cannot “see” the geometry: we use a systematic method.

Spherical-wrist robots (axes 4, 5, 6 concurrent, the case of most industrial 6R): the problem decouples. The first 3 axes fix the position of the centre of the wrist (θ₁, θ₂, θ₃: position problem), the last 3 fix the orientation (θ₄, θ₅, θ₆: orientation problem, identical to the extraction of Euler angles of step 2).

Key terms

• MGI
• aspect / posture
• Paul method
• decoupling

Watch the inverse kinematics of the SCARA live →

7.1 Two ways of representing the same configuration

• Joint space: space of the variables (q₁, …, q_n), of dimension n (4 for a SCARA, 6 for a 6R). A configuration of the robot is a point Q.
• Operational space: space of the situation of the tool (position x, y, z + orientation). The same configuration is a point E.
• Q and E represent the same configuration. During a movement, Q describes a joint trajectory and E an operational trajectory (the one programmed by the roboticist): they correspond to the same task.
• For a serial robot: to one Q corresponds one single E (MGD), but to one E may correspond several Q_i, Q_j … (MGI, aspects).

7.2 Exercise of course C1: 2-DOF SCARA

7.3 Joint limits and workspace

Each variable is bounded: q_i^min ≤ q_i ≤ q_i^max. If the limits are fixed, the accessible joint space is a block (rectangular parallelepiped). Its image by the MGD is the workspace (space reachable by the tool centre). The more pivots the robot has, the more complex its borders are (crescent or mushroom shapes). We generally only represent the position, without orientation constraint.

7.4 The different forms of movement

Key terms

• joint space
• operational space
• workspace
• joint stops

(Complement from Khalil–Dombre, essential to understand singularities and the link speeds/forces.)

8.1 The direct kinematic model

It links the operational speeds to the joint speeds:

X˙ = J(q) q˙, J_ij = ∂f_i(q)∂q_j (matrix m × n)

For a small robot, J is obtained by differentiating the MGD. For a robot with more than 3 DOF, we use the kinematic Jacobian: the column k is

j_k = σ_k a_k + σ̄_k (a_k ∧ L_k,n)σ̄_k a_k

where a_k is the unit vector of z_k and L_k,n = O_kO_n: a pivot creates a speed a_k ∧ L_k,n and a rotation a_k; a slider creates a translation a_k.

8.2 Singularities

When the rank of J drops, the robot locally loses the possibility of moving in some directions. For a square J: singularity if det J = 0; redundant robot: det(JJ^T) = 0. Example Stäubli RX-90: det(³J₆) = −C₃D₃RL₄S₅(S₂₃RL₄ − C₂D₃), hence 3 singularities: C₃ = 0 (stretched arm), S₂₃RL₄ − C₂D₃ = 0 (wrist on axis 1, shoulder), S₅ = 0 (axes 4 and 6 aligned, wrist).

8.3 The inverse kinematic model

• Regular case (J square and invertible): q˙ = J⁻¹(q) X˙.
• Redundant robot: pseudo-inverse q˙ = J⁺ X˙ with J⁺ = J^T(JJ^T)⁻¹ (solution of minimal norm).
• Near a singularity, J⁻¹ explodes (enormous joint speeds): we use damped least squares q˙ = J^T(JJ^T + λ²I)⁻¹ X˙.

Link with statics: the motor torques needed to exert a force F on the environment are Γ = J^T(q) F.

Key terms

• Jacobian matrix
• singularity
• pseudo-inverse
• damped least squares

9.1 The principle

Knowing the start and the arrival is not enough: we must decide how the position evolves in time. For a joint movement from qⁱ to q^f:

q(t) = qⁱ + r(t) D, D = q^f − qⁱ, r(0) = 0, r(t_f) = 1

The function r(t) defines the motion law. It is chosen according to the limits of the motor: maximum speed V_max (given by the gearmotor) and maximum acceleration a (given by the torque, therefore by the motor current).

9.2 Bang-bang law

Zero speed at the start and at the end. First half at constant acceleration, second half at constant deceleration. The maximum speed is reached at mid-course, the mean speed is V_max/2. The position is made of two parabolas tangent to the horizontal at the start and at the end.

q˙_max = 2|D|t_f, q¨_max = 4|D|t_f², t_f = max( 2|D|k_v , 2 |D| / k_a )

Continuous in position and in speed, discontinuous in acceleration.

9.3 Speed trapezoid law (the most used)

Three phases: (1) constant acceleration a up to V_max; (2) constant speed V_max; (3) constant deceleration down to the stop. Adapted to large displacements (V_mean ≃ V_max). It saturates both speed and acceleration: it is the minimum-time law.

9.4 Acceleration trapezoid law (limited jerk)

Seven phases: increasing, constant, decreasing acceleration; constant speed; then the three symmetrical phases. The acceleration is continuous: we limit the sudden variations of current in the motor, which increases its life (used on cobots, e.g. Fanuc CR-15iA).

9.5 Sine-squared speed law

Phase 1: V(t) = V_max sin²(ωt) up to sin²(ωt) = 1; phase 2 at constant speed; phase 3 symmetrical. The acceleration (V_max ω sin 2ωt) is continuous (“sine–ramp–sine” profile).

Comparison trapezoid / sin² (same acceleration duration T):

• at identical maximum acceleration A_max: trapezoid V = TA_max; sin² V = (2/π)TA_max ≈ 0.64 TA_max, that is 36 % less;
• but the smoothness of the sin² (no current discontinuity) allows to tolerate accelerations up to 100 % larger; the speed reached is then at least equal to the one of the trapezoid, and the motor is better protected.

9.6 Degree-5 polynomial interpolation (Khalil–Dombre)

For a movement continuous in position, speed and acceleration (class C²), with zero speeds and accelerations at the ends:

r(t) = 10(t/t_f)³ − 15(t/t_f)⁴ + 6(t/t_f)⁵, q˙_max = 15|D|8t_f, q¨_max = 10|D|√3 t_f²

9.7 Multi-axis movement: homothetic trapezoids

For a point-to-point joint displacement, we want all the axes to start and stop at the same time.

9.8 Joint or operational?

Key terms

• bang-bang
• speed trapezoid
• jerk
• homothetic trapezoids
• minimum time

(Synthesis of chapters 1.4 and 3 of Khalil–Dombre.)

10.1 The dynamic model

It links the motor torques to the movements:

Γ = A(q) q¨ + C(q, q˙) q˙ + Q(q) + F_vq˙ + F_s sign(q˙) + J^TF_e

• A(q): inertia matrix (n × n, symmetric positive definite);
• C(q, q˙)q˙: centrifugal and Coriolis effects;
• Q(q): gravity;
• F_v, F_s: viscous and dry friction; J^TF_e: force exerted on the environment.

Two computation methods: Lagrange (from the kinetic energy E and the potential energy U: Γ_i = d/dt(∂L/∂q˙_i) − ∂L/∂q_i, L = E − U) and Newton–Euler (recursive, numerically more efficient).

10.2 Control

• Joint PID (classic control, axis by axis): Γ = K_p(q^d − q) + K_d(q˙^d − q˙) + K_i∫(q^d − q) dt. Simple, used in industry, but the performance varies with the configuration.
• Non-linear decoupling control (computed torque): Γ = Â(q) w + Ĥ(q, q˙) with w = q¨^d + K_d(q˙^d − q˙) + K_p(q^d − q). We compensate the dynamics of the robot, which then behaves like n decoupled double integrators.

Key terms

• inertia matrix
• Coriolis
• PID
• computed torque