Robot Manipulator Control - SJTU

Robot Manipulator Control

Hesheng WangDept. of Automation

Introduction• Industrial robots work based on the

teaching/playback scheme– Operators teach the task procedure to a robot– The robot plays back (execute the motion)

• Intelligent robots work without teaching– Robots plan its motion automatically– Robots execute the motion planned

• To carry out tasks successfully, a robot needs to input proper inputs to their joint actuators to execute the motion taught/planned.

Problem Definition• Robot Control is a problem of designing a proper input to

force a robot to carry out a task or a motion as precisely as possible.

• Robot control problems can be classified as follows according to the requirements:– Position control: Control a robot to a desired position– Trajectory control: Control a robot to follow a desired geometric

path or a time-varying trajectory– Force control: Control a robot to apply a desired force to the

environment or other objects– Hybrid position/force control: Control position and force of a

robot– ….

Controller

• To control motion of a robot based on the information collected by the sensors and instructions.

RobotControllerMotion instruction

Sensor Feedback

Control Method• Open-loop Control: Directly compute the

motion input from the motion command without using feedback information.

- simple- big errors- no robustness

robotControllerreference

Control Method• Closed-loop control: Control the robot based on the

instruction and the sensor feedback.- High accuracy- Robust

RobotControllerreference

Sensor feedback

Review of Stability Theory

Nonlinear State-Variables SystemsThe robot dynamics has the form:

τqGqqCqqH )(),()(

From dynamic equation:

τqHqGqqCqHq )()(),()( 11

xy

τxHxGxxCxHx

xxx

01)(

0)(),()(

1

11211

12

21

State equation of robot dynamics

qyqxqx , , 21 Let

Nonlinear State-Variables Systems

In general, the state equation of nonlinear systems:

)()(

ux,hyux,fx

State variables vectorSystem output

System input

Positive-Definite Matrix

• All eigenvalues of a positive-definite matrix are positive.• All eigenvalues of a positive-semidefinite matrix are non-negative• All eigenvalues of a negative-definite matrix are negative.• All eigenvalues of a negative-semidefinite matrix are non-positive

0 and 0 if definite-positive is matrixn nAn

xxHxx

H nT R,

nT R,

xHxxH

0 if tesemidefini-positive is matrixn nAn

0 and 0 if definite-negative is matrixn nAn

xxHxx

H nT R,

nT R,

xHxxH

0 if tesemidefini-negative is matrixn nAn

Equilibrium Point• Equilibrium point: A state xe of a system in

which the velocity of the system is equal to zero.

)0 satisfiespoint mequilibriu the 0xLet

,tf(xe

) :System f(x,tx

Equilibrium point of a system may not be unique

Ball staying at the bottomBall staying on the top

Lyapunov Stability

• Xe is stable in the sense of Lyapunov at t0, if starting close enough to xe at t0, the system will always stay close to xe at later time.

0e0e00

0e

t t,x-x(t) then ),t,(x-)x(t if such that )t,( positive a exists there0,given any for

if at t Lyapunov of sense in the stable is x:Definition

xe

System trajectory

Lyapunov Stability

It should be noted that stability does not mean that the state of the system will go to the equilibrium point as time goes to the infinity.

xe

Stable with or without friction

Ball staying on the top

xe

Instable

Asymptotic StabilityXe is asymptotically stable if it is both stable and

convergent.

ee t xxxx )(lim then ,)(t ifsuch that positive a exists therei.e.t0

xe

Global Exponential Stability

te ea

a

xxxxx

x

)0(-(t) (0),for such that 0 and

0exists thereif stablelly exponentiaglobally is

e

e

e-(t) xx

tTrajectory of the error

Exponential function

Lyapunov Theorem• Given the nonlinear system:

with an equilibrium point at the origin (x=0). Let N be a neighborhood of the origin. Suppose that x0 is within the region N. The origin is stable in the sense of Lyapunov if there exists a scalar function V(x) such that

(1) V(x) is positive-definite in region N(2) is negative-definite or negative-semidefinite in region N

The origin is asymptotically stable if (1) V(x) is positive-definite in region N(2) is negative-definite in region N

0(0) ),( xxxx f

)(xV

If the N is the entire state space, the stability is global; otherwise, the stability is local

)(xV

LaSalle Theorem• Given the nonlinear system:

with an equilibrium point at the origin (x=0). Suppose that there exists a scalar function V(x) such that

(1) V(x) is positive-definite(2)The system will be convergent to the maximum invariant

set within the region defined by . If the maximum invariant set contains the origin only, the origin is asymptotically stable.

0(0) ),( xxxx f

0)( tV

0)( tV

Stability Analysis

0)(lim then

,)(lim

if and continuousuniformly is f(t) If :

t

0t

tf

aadttft

LemmaBarbalat

)f(t-f(t) then ),( t-t if 0 tand 0any tfor such that 0)( exists there0,any

for if continuousuniformly is f(t)function A :

111

continuity Uniform

A sufficient condition for uniform continuity: A differentiable function f(t) is uniformly continuous if its derivative is bounded.

Position Control• Position control is a problem of properly

designing joint inputs of a robot manipulator so as to drive it to a desired configuration (position and orientation)– The desired position is a constant vector– The desired velocity and acceleration are zero.

• Position control can be classified into– Position control in joint space– Position control in Cartesian space.

Joint Servo System

• Computer or MPU: carry out the necessary computation in a control law

• D/A converter: Transform digital output of the computer to analogical voltages

• Amplifier: Amplify the input voltage to the actuator.• Actuator: Drive the link• Sensors (encoder, etc): Measure the joint angle and velocity.

ComputerD/A

Converter Amplifier Actuator Link

Sensor

Reference

Actuator Dynamics An actuator usually delivers torque to a robot link

via a reduction mechanism.

iL ,

linkMotor

Reduction gears

Motor shaft

im ,

Gear ratio: s

(1) ii s

Motor torque:

(2) s

H Lmm

Torque consumed by the reduction mechanismLoad of the link

Dynamics of DC motor

Consider the equivalent circuit of a DC motor

constant alproportion uemotor torq :K(3) Kim

+V

i

R L

R: Armature resistanceL: Armature inductanceV: Armature voltage

Applying Kirchnoff’s voltage law

(4) VRiKdtdiL ib

Kb: back electromotive force (emf) constant

Neglecting the inductance and considering (1) :

(5) VRisK ib

Robot Dynamics with Actuator Dynamics

Combining (2) and (5) leads to the relationship between motor input voltage and the torque delivered to the link:

ib

imL sR

KKsHVRKs 22

Substituting this into the robot dynamics:

uqGqqqSqHBqqHH )()),()(21()( 0 m

i

biii

nn

nnT

imim

RKKsdiag

VR

sKVR

sKVR

sKsHdiag

2

0

22

221

1

11

2

),...,,(

B

u

HIf si are very large,

uqGqBqH )(0 m

If there is no gravity:uqBqH 0m Linear system

PD Feedback ControlControl objective: Control the position q of the robot to a

desired position qd.

qBqAu -- :Law Control

matrixgain Velocity :matrixgain Position :

errorPosition :

BA

qqq d

The closed-loop dynamics:

qBqAqGqqqSqHqqH Δ)()),()(21()(

The equilibrium point: )(1 qGAq eΔ

At the equilibrium point, the position error is not zero. The error is called offset error

PD Feedback ControlStability of the equilibrium point

)(21)(

21)( qqqqqHq UtV TT

Introduce the following candidate of Lyapunov functionGravitational Potential energy

qqqAqqHq )()()( UtV T

)()( qGqq

UForm the closed-loop dynamics and noting

0)( qBq TtVV(t) is a Lyapunov function of the closed-loop dynamics, so the equilibrium is stable. However, the position error does not go to zero

The position goes to zero when there is no gravity force

Offset Error

• How to kill the offset error– Design a robot such that the gravity term is

zero– Increase the gain- high-gain control– Having a gear-ratio reduction mechanism

leads to high gain control– Compensate for the gravity force on-line

)(1 qGAq ΔThe PD controller leads to an offset error:

PD Feedback Plus Gravity Compensation

)(-- :Law Control qGqBqAu

The closed-loop dynamics:

qBqAqqqSqHqqH Δ)),()(21()(

The equilibrium point: )0,0( qq Δ

Gravity compensator

The gravity force is calculated on-line so as to cancel its effect

This controller was developed by Arimoto and Takegaki, 1981

Convergence of Position Error

qΑqqqHq TTtV21)(

21)(

qAqqHqqHq )(

21)()( TtV

Introduce the following candidate of Lyapunov function

Form the closed-loop dynamics and noting

0),()( qBqqBqqqqSq TTTtV

,0lim ,0lim

means Theorem LaSalle Therefore, 0).0,(only contains 0set with invariant the,0 and dynamics loop-closed theFrom

tt

qq

qq

(t)V(t)V

PID Feedback Control

t

dt0321 - :Law Control qKqKq-Ku

gain)-(Imatrix gain Integral :gain)-(Dmatrix gain y velocit:

gain)-(Pmatrix gain error position :

321

KKK

By introducing I-feedback, it can kill the offset error in PD feedback control

However, the PID controller only gives local asymptotic stability. The proof is very complicated

Position Control in Cartesian Space

• Why?– Most tasks are specified in a Cartesian space.

We need to position the end-effector to desired position and orientation

– Control in the joint space will lead to offset errors due to errors in robot kinematics models.

• Difference from joint space control– Directly use feedback of position in Cartesian

space.

Position Control in Cartesian Space

)(-)(- :Law Control qGqBxAqJu T

spaceCartesian in error position :

Jacobian Robot :)(dxxx

qJ

qBxAqJqqqSqHqqH ΔT )()),()(21()(

xΑxqqHq TTtV21)(

21)(

qqJxx

qqJx

)( ectorconstant v a is

and )(

d0

),((

)()(21)()(

qBqqqqSqqBq

xΑqJqqHqqHq

T

TT

TTtV

Then, From LaSalle’ Theorem, we can further prove 0lim xt

Position of the end-effector Desired positionThe closed-loop dynamics

Consider the positive-definite function:

Therefore, The system is stable.

Trajectory Tracking Control• Trajectory tracking control is a problem of

designing proper joint inputs for a robot to trace a pre-specified trajectory presented by

• In trajectory tracking, the nonlinear inertia, centrifugal and Coriolis forces play important roles. Their compensation is one of the key issues in controller design

onaccelerati desired :)( velocitydesired :)(position desired :)(

)( and )(),(

ttt

ttt

ddd

ddd

qqq

qqq

Computed Torque Method• Linearization of the nonlinear dynamics

– Change the nonlinear system to a linear system by state feedback.

– The idea is to use on-line measurement of position and velocity to compensate for the nonlinear forces.

(1) )()),()(21()( τqGqqqSqHqqH

The robot dynamics

Design the joint input as follows:(2) ')()),()(

21( τqGqqqSqHτ

Substituting it to the dynamics:

(3) ')( τqqH

A new input

Computed Torque MethodDesign the new input as follows:

(4) )(' uqHτ Substituting this into eq. (3) in the previous slide:

New input

(5) uq This is a linear system. It is easy to design the following controller:

(6) )()()()( 21 tttt d qKqKqu

),()()( where ttt dd qqq )()()( ttt dd qqq The closed-loop system is

(7) 0)()()( 21 ttt qKqKq

(7) Is a second order system. It is asymptotically stable if K1 and K2 and positive-definite. Then from (2), (3) and (5), joint input is

(8) ))()()()(()()),()(21( 21 tttd qKqKqqHqGqqqSqHτ

Computed Torque Method

• It uses real trajectory of a robot manipulator to compensate for its nonlinear forces (centrifugal and Coriolis forces) on-line.

• The on-line compensation requires an efficient algorithm to solve the inverse dynamics in real-time.

• The recursive Newton-Euler formulation can solve the inverse dynamics in real-time.

• The measurement errors of joint positions and velocities will greatly affect the control performance

))()()()(()()),()(21( 21 tttd qKqKqqHqGqqqSqHτ

Controller: