**5. The inner-loop control design**

In this part, we will present the control system design of the camera pose control and the inner joint control loops for both the camera movement adjustment and the tool manipulation, which have been introduced in Section 4. A simulation scenario is also presented in this section.

We express an equation for the 6 DOF manipulator including the dynamics of the robot manipulator and the actuators (DC motor) in the following simplified form [28]:

$$(D(q) + f)\ddot{q} + \left(C(q, \dot{q}) + \frac{B}{r}\right)\dot{q} + \mathbf{g}(q) = u \tag{19}$$

where *D q* and *C q*, *<sup>q</sup>*\_ are **<sup>6</sup>** � **<sup>6</sup>** inertial and Coriolis matrices respectively. *<sup>J</sup>* is a diagonal matrix expressing the sum of actuator and gear inertias. *B* is the damping factor, and *r* is the gear ratio, **g** *q* is the term for potential energy, *<sup>u</sup>* is the 6 � 1 input

### **Figure 9.**

*The high precision camera sensor. Note: Blue are signals used in the control system of Figure 7 and red are signals used in the control system of Figure 8.*

*Role of Uncertainty in Model Development and Control Design for a Manufacturing Process DOI: http://dx.doi.org/10.5772/intechopen.104780*

vector, and *q* is the 6 � 1 generalized coordinates (in this paper, *q* is the 6 � 1 joint angle vector).

We can simplify Eq. (19) as follows:

$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + h(\mathbf{q}, \dot{\mathbf{q}} \,) = \mathbf{u} \tag{20}$$

with

$$M(q) = D(q) + J \tag{21}$$

$$h(\mathbf{q}, \dot{\mathbf{q}} \mid) = \left( C(q, \dot{q}) + \frac{B}{r} \right) \dot{q} + \mathbf{g}(q) \tag{22}$$

If we transform the control input as following:

$$\mathbf{u} = \mathbf{M}(\mathbf{q})\mathbf{v} + h(\mathbf{q}, \dot{\mathbf{q}}) \tag{23}$$

where *v* is a virtual input. Then we substitute for u in Eq. (20) using (23), and since *M q*ð Þ is invertible, we will have a reduced system equation as follows:

$$
\ddot{q} = v \tag{24}
$$

This transformation is so-called feedback linearization technique with the new system equation given in Eq. (24). This equation represents 6 uncoupled double integrators. The overall feedback linearization method is illustrated in **Figure 10**. In this control block diagram, we force the joint angle *q* to follow the target joint angle *qR* so that the output pose, *P*, can follow the target pose, *P*. *P, P*, *q*, and *qR* are all vectors with six elements (each element corresponds to a joint position or angle). The Nonlinear interface transform the linear virtual control input *v* to the nonlinear control input u by using Eq. (23). The output of the manipulator dynamic model, the joint angles, *q*, and their first derivatives, *q*\_, are utilized to calculate *M q*ð Þ and *h*ð Þ q, q\_ in the Nonlinear interface. The linear joint controller is designed using Youla parameterization technique [9] to control the nominally linear system in Eq. (24).

The design of a linear Youla controller with nominally linear plant is presented next. Since the transfer functions between all inputs to outputs in (24) are the same and decoupled, we can first design a SISO (single input and single output) controller and use the multiple of the same controller for a six-dimension to obtain the MIMO (Multiple Input Multiple Output) version. In other words, we first design a controller *GcSISO* that satisfies:

$$
\sigma\_{\text{SISO}} = \ddot{q}\_{\text{SISO}} \tag{25}
$$

where *vSISO* is a single input to a nominally linear system and *q*€*SISO* is the second order derivative of a joint angle. The controller system in **Figure 10** can be then written as:

$$\mathbf{G}\_{\mathbf{c}\_{\mathfrak{pr}}} = \mathbf{G}\_{\mathbf{c}\text{SISO}} \cdot I \tag{26}$$

where *I* is a 6 � 6 identity matrix. We can design the SISO controller *Gc* using Youla parameterization technique [48]. The transfer function of the SISO nominally linear system from (24) is:

$$G\_{\rm p\_{SISO}} = \frac{1}{\mathfrak{s}^2} \tag{27}$$

Note that *GpSISO* has two BIBO (Bounded Input Bounded Output) unstable poles at origin. To ensure internal stability of the feedback loop, the closed loop transfer function, *TSISO*, should meet the interpolation conditions [39]:

$$T\_{\rm SISO}(\mathfrak{s}=\mathbf{0}) = \mathbf{1} \tag{28}$$

$$\left. \frac{d T\_{\rm SISO}}{d s} \right|\_{s=0} = \mathbf{0} \tag{29}$$

We compute a Youla transfer function: *YSISO*, using the following relationship,

$$T\_{\rm SISO} = Y\_{\rm SISO} G\_{p\_{\rm SISO}} \tag{30}$$

The *TSISO* is designed so that it satisfies the conditions in (28) and (29). The sensitivity transfer function, *SSISO*, is then calculated as follows:

$$\mathbf{S\_{SISO}} = \mathbf{1} - T\_{\text{SISO}} \tag{31}$$

Without providing the design details, we refer the interested reader to [38], the closed-loop transfer function should be in the following form to satisfy the interpolation conditions:

$$T\_{\text{SISO}} = \frac{\left(\mathbf{3\pi} + \mathbf{1}\right)}{\left(\mathbf{z}\mathbf{s} + \mathbf{1}\right)^{3}}\tag{32}$$

Where *τ* specifies the pole and zero locations and represents the bandwidth of the control system. We can tune *τ* so that the response can be fast with less-overshoot.

Then we can derive *GcSISO* from relationships between the closed-loop transfer function, *TSISO*, the sensitivity transfer function, *SSISO*, and the Youla transfer function, *YSISO*, in Eqs. (30)–(32):

$$Y\_{\rm SISO} = T\_{\rm SISO} G\_{p\_{\rm SISO}}^{-1} = \frac{s^2(3\pi^2 s + 1)}{\left(\pi s + 1\right)^3} \tag{33}$$

$$S\_{\rm SISO} = \mathbf{1} - T\_{\rm SISO} = \frac{s^2(\tau^3 s + 3\tau^2)}{\left(\pi s + \mathbf{1}\right)^3} \tag{34}$$

*Role of Uncertainty in Model Development and Control Design for a Manufacturing Process DOI: http://dx.doi.org/10.5772/intechopen.104780*

$$\mathbf{G}\_{\rm cISO} = \mathbf{Y}\_{\rm SISO} \mathbf{S}\_{\rm SISO}^{-1} = \frac{\mathbf{3}\mathbf{r}^2 \mathbf{s} + \mathbf{1}}{\mathbf{r}^3 \mathbf{s} + \mathbf{3}\mathbf{r}^2} \tag{35}$$

From Eq. (35), we can compute a MIMO controller as follows:

$$G\_{\mathfrak{c}\_{\mathfrak{h}}} = \frac{\mathfrak{Z}\mathfrak{r}^{2}\mathfrak{s} + \mathbf{1}}{\mathfrak{r}^{3}\mathfrak{s} + \mathfrak{Z}\mathfrak{r}^{2}} \cdot I\_{6 \times 6} \tag{36}$$

Eq. (36) provides the desired controller, which is used as the joint controller, as shown in **Figure 10**. This configuration is precisely the inner joint control loop in both the visual and the manipulator systems as shown in **Figures 7** and **8**.

**Figure 11** shows the simulation results for the case with no disturbance. The target

position and the orientation of the end-effector are selected to be *X Y Z* 2 6 4 3 7 5 ¼ 1*:*7157*m* 1*:*0191*m* 0*:*7518*m* 2 6 4 3 7 5

and *n s a* 2 6 4 3 7 5= �0*:*425 0*:*87 0*:*25 0*:*8361 0*:*2714 0*:*4767 0*:*3469 0*:*4116 �0*:*8428 2 6 4 3 7 <sup>5</sup>, where *<sup>X</sup>*, *<sup>Y</sup>*, *<sup>Z</sup>* � �*<sup>T</sup>* is the absolute position

coordinate of the center of the end effector in the inertial frame and *n*, *s*, *a* represent respectively the end-effector's directional unit vector of the yaw, pitch and roll in the inertial frame. Therefore, the corresponding target angles of rotations are *qR* ¼ ½ � 30°, 60°, �45°, 15°, 45°, 90° **.** For this simulation, we have designed the control system with the bandwidth of 100 *rad=s*. In the following three plots, solid lines represent the responses for the end-effector position of each joint and the end-effector orientation respectively, and the dashed lines are the targets. Specifically, the orientation response of the end-effector is the vector that tangent to the curve in the second plot at each point in **Figure 11**. The simulation results show that all responses of the controlled system will be able to reach their final/steady state values within 0.1 second with no (or little) overshoots.

**Figure 11.** *The simulation results for the end-effector response to an arbitrary selected trajectory.*
