Decentralized Joint Control#

The equation of motion of a manipulator without end-effector contact force and any joint friction is

B(q)q¨+C(q,q˙)q˙+g(q)=τ

Let qm=[θm1,θm2,...θmn]T denote the vector of all corresponding DM motor angles. The transmissions establish a relationship:

Krq=qm

where Kr=diag(kr1,kr2,...,krn) is a diagonal matrix, and each diagonal element kri is the gear ratio of joint i. Let τm=[τm1,τm2,...,,τmn]T denote the vector of all motor torques, one can write

τm=Kr1τ

Substituting ([equ.motor_angle]{reference-type=”ref” reference=”equ.motor_angle”}) and ([equ.motor_torque]{reference-type=”ref” reference=”equ.motor_torque”}) into the manipulator dynamics ([equ.dyn]{reference-type=”ref” reference=”equ.dyn”}) leads to

Kr1B(q)Kr1q¨m+Kr1C(q,q˙)Kr1q˙m+Kr1g(q)=τm

We have previously analyzed elements in matrix B(q), one can write it as

B(q)=B+ΔB(q)

where B is the diagonal matrix whose constant elements represent the average inertia at each joint, and elements in ΔB(q) represent the matrix of non-diagonal part. Substituting ([equ.bmat]{reference-type=”ref” reference=”equ.bmat”}) into ([equ.dyn2]{reference-type=”ref” reference=”equ.dyn2”}) leads to

Kr1BKr1Imq¨m+Kr1ΔB(q)Kr1q¨m+Kr1C(q,q˙)Kr1q˙m+Kr1g(q)D=τm

The above equation ([equ.decouple_model]{reference-type=”ref” reference=”equ.decouple_model”}) will lead to the following diagram.

../_images/decentralized_control3.jpeg

Fig. 71 Dynamics diagram of a motor-driven manipulator.#

As shown in Fig. 1{reference-type=”ref” reference=”fig:3”} and ([equ.decouple_model]{reference-type=”ref” reference=”equ.decouple_model”}), if we don’t consider D, then

Imq¨m=τm,

Note that in the above equation Im is also a diagonal matrix. This means that if we don’t consider the complex D, each joint is a single-in-single-output system:

Imθ¨m=τm

for each joint motor and we can control each joint individually. However, D in ([equ.decouple_model]{reference-type=”ref” reference=”equ.decouple_model”}) is a coupling term that prevents us from doing so. To address such, in the following, we still consider the control each joint as a single-in-single-out system, but consider D as a disturbance to each joint.

Single-Joint Control Diagram#

For each joint (we omit the joint index i below), considering ([equ.single_joint_dyn]{reference-type=”ref” reference=”equ.single_joint_dyn”}) and the motor model in the previous lecture (Lecture 19), we have

Imθ¨m+D=τm=Imθ¨m+ktRa(DRakt)=τm=ktia=kt(vakvθ˙m)Ra=kt(Gvvckvθ˙m)Ra

Here, D is the disturbance torque in ([equ.decouple_model]{reference-type=”ref” reference=”equ.decouple_model”}) applied to each joint, kt is the torque constants; ia is the armature current; va is the vector of armature voltage; Ra is the armature resistance; kv is the back EMF constant; vc is the voltage of the servomotor, and Gv is amplifier. Those motor parameters have been shown in previous lecture. In the following, we will suppose vc=va, i.e., Gv=1, for notation simplicity, and so we will directly treat va as the servo motor input voltage. But the analysis also holds for any Gv.

The above ([equ.motor]{reference-type=”ref” reference=”equ.motor”}) corresponds to the following diagram:

../_images/motor_model3.jpeg

Fig. 72 Diagram of a motor-driven manipulator, with coupled joint terms treated as disturbance.#

In the above diagram, the closed loop is due to the DC motor model itself. We write input (Va) to output (θm) transfer function $G(s)=Θ(s)Va(s)=kms(1+Tms)$

Θ˙(s)Va(s)=ktRaIms1+ktRaImskv=km1+Tms

Minimal Control Background

For ease of the reader, we provide the following minimal background of control design. A typical closed-loop control diagram is shown below. Here, θr is the reference input; θm is the output; D is the disturbance; G(s) is the transfer function of the plant to be controlled; C(s) is the controller; H(s) is the backward pass transfer function (usually a constant due to sensor). The goal of control design is to find C(s) such that θm is able to track θr while D has as little effect on θm as possible.

../_images/fd_control2.jpeg

Fig. 73 A typical control system diagram#

For the above control system, the forward path transfer function is

P(s)=C(s)G(s)

The backward path transfer function is $H(s)$ The open loop transfer function is

C(s)G(s)H(s)

The input-to-output transfer function is

Θm(s)Θr(s)=C(s)G(s)1+C(s)G(s)H(s)

The disturbance-to-output transfer function is

Θm(s)D(s)=G(s)1+C(s)G(s)H(s)

The input (θr) to output (θm) response of the closed-loop system will be determined by the roots of the characteristics equation $1+C(s)G(s)H(s)=0$

Background of final value theorem

If a continuous signal f(t) has its Laplace transformation F(s), then the final value theorem states

limtf(t)=lims0sF(s)

Single Joint Position feedback#

The single joint motor system in Fig 2{reference-type=”ref” reference=”fig:motor_model2”} is the plant we want to control. The plant transfer function G(s) is ([equ.motortf]{reference-type=”ref” reference=”equ.motortf”}), rewritten below

G(s)=kms(1+Tms)

We want to design a proportional-integral controller

CP(s)=KP1+sTPs

with KP and TP are design variable.

The control diagram thus is shown below (note that the backward pass constant kTP is fixed).

../_images/p_control2.jpg

Fig. 74 Control diagram of position decentralized joint space control#

The transfer function of the forward path is

P(s)=G(s)CP(s)=kmKP(1+sTP)s2(1+sTm)

The transfer function of the backward pass is

H(s)=kTP

The root locus can be plotted as a function of the gain of the position loop kmKPkTPTP/Tm. Here, we derive into two cases to plot the root locus.

  • If TP<Tm, the root locus is shown as below. Thus, the system is inherently unstable.

    ../_images/p_control_rl_1.jpg

    Fig. 75 Root locus when TP<Tm#

  • If TP>Tm, the root locus is shown as below. As TP increases, the absolute value of the real part of the two roots of the locus tending towards the asymptotes increases too, and the system has faster time response.

    ../_images/p_control_rl_2.jpg

    Fig. 76 Root locus when TP>Tm#

The closed-loop input/output transfer function is

Θm(s)Θr(s)=P(s)1+P(s)H(s)=kmKP(1+TPs)kTPkmKP(1+TPs)+s2(1+sTm),

and the characteristic equation can be factorized into the following form

kTPkmKP(1+TPs)+s2(1+sTm)=(ωn2+2ζωns+s2)(1+sτ)

where ωn and ζ are the natural frequency and damping ratio of the complex poles: (ζωn,±j1ζ2ωn), and 1/τ is the real pole. These poles correspond to the ones in the root locus in the above figures.

The closed-loop disturbance/output transfer function is

Θm(s)D(s)=RaKtG(s)1+P(s)H(s)=RaKtkmskTPkmKP(1+TPs)+s2(1+sTm)

If Θr(t)=0, based on the final value theory, we have

limtθm(t)=lims0sΘm(s)=lims0sΘm(s)D(s)D(s)

thus,

limtθm(t)={0ifD(t)is constant signalRaKt1KPkTPifD(t)is ramp signal, such as D(t)=vt 

Hence, the controller can cancel the effect of constant disturbance, and the quantity

KPkTP

can be interpreted as the disturbance rejection factor for velocity (or higher-order) disturbance. Increasing KP can help with reducing the effect of D, but too high KP can lead to unacceptable oscillations of the output, as implied from the root locus.

Single-Joint Position and Velocity Feedback#

To control the motor system in Fig. 2{reference-type=”ref” reference=”fig:motor_model2”}, we use both position and velocity controller $CP(s)=KPandCV(s)=KV1+sTVswhereK_p,K_v,andT_v$ are the design variables.

The control diagram is shown below (Note that kTV and kTP in the backward pass are fixed).

../_images/pv_control.jpeg

Fig. 77 Control diagram of position and velocity control#

which can be further reduced into the following diagram:

../_images/pv_control2.jpeg

Fig. 78 Corresponding to Fig. 7{reference-type=”ref” reference=”fig.pv_control”}, the equivalent control diagram#

The transfer function of the forward path is

P(s)=C(s)G(s)=kmKPKV(1+sTV)s2(1+sTm)

The transfer function of the backward path is

H(s)=kTP(1+skTVKPkTP).

For simplicity, one can design

Tm=Tv,

then that the poles of the closed-loop system move on the root locus as a function of the loop gain kmKVkTV is shown in the following figure:

../_images/pv_control_rl_1.jpg

Fig. 79 Root locus#

By increasing the position gain KP, it is possible to confine the closed-loop poles into a region of the complex plane with large absolute values of the real part.

The closed-loop input/output transfer function is

Θm(s)Θr(s)=C(s)G(s)1+C(s)G(s)H(s)=1kTP1+skTVKPkTP+s2kmKPkTPKV=1kTP1+2ζsωn+s2ωn2

It can be recognized that, with a suitable choice of the gains, it is possible to obtain any value of natural frequency ωn and damping ratio ζ. Hence, if ωn and ζ are given as design requirements, the following relations is

KVkTV=2ζωnkm
KPkTPKV=ωn2km

For given transducer constants kTP and kTV, KV and KP can be chosen. The closed-loop disturbance/output transfer function is

Θm(s)D(s)=RaktG(s)1+P(s)H(s)=Raktkms(1+sTm)(s2+kmKVkTVs+kmKPKVkTP)

which shows that the disturbance rejection factor is

If Θr(t)=0, based on the final value theory, we have

limtθm(t)=lims0sΘm(s)=lims0sΘm(s)D(s)D(s)

thus,

limtθm(t)={0ifD(t)is constant signalRakt1KPkTPKVifD(t)is ramp signal, such as D(t)=vt 

Hence, the controller can cancel the effect of constant disturbance, and the quantity

KPkTPKV

can be interpreted as the disturbance rejection factor for velocity (or higher-order) disturbance. Increasing KP and KV can help with reducing the effect of D.