In this section, we compare only the best combination of parameters for each of the two studied methods. Finally, we selected one of them to be used during the experimental validation proposed in Section 5. Recall that at the beginning of this work, we stated some requirements that the selected approach should fulfill. The human and the robot should interact naturally, and the robot adaptation should last one second at most. Furthermore, in the future, we aim to consider the contextual information of the industrial processes surrounding the proposed collaborative task. Hence, it would be desirable that the method to infer the human's intention could deal with heterogeneous data, not only temporal sequences.

The selected combination in the case of 1NN + DTW ensured an inference time of 0.8 s and a performance score of 97.99%, which was rather good. It corresponded to using independent DTW, a window of 0.7 s, and sub-sampling of the data to 2% of the window's size (see Section 4.2.2 for more detail). When using GPLVM + SVM, the selection was GPLVM without optimization, a window of 0.7 s, and 10 latent variables. This approach resulted in an F1-score of 98.14% and an inference time of 0.85 s (see Section 4.3.2 for more detail). As we can see, the quantitative differences between the two alternatives were negligible. Therefore, to provide more useful insights into the comparison between 1NN + DTW and GPLVM + SVM, we analyzed them using more qualitative measures. They were extracted from the hands-on experience acquired along the developed work and were meant to ease the selection procedure.

be used to learn not only the physical interactions but also the contextual information. 1NN + DTW, however, cannot deal with other data apart from sequential. It would be necessary to use a second kNN model with another metric (e.g., Euclidean) and then apply ensemble learning techniques.

Based on the previous analysis, we selected GPLVM + SVM. In particular, we proposed to use GPLVM without optimization during the inference, a sampling window of 0.7 s, and 10 latent variables. The first reason was that we thought GPLVM's generalization capabilities could help us in future works. In robotics, especially in industrial environments, data are presented in heterogeneous ways: sequential data, digital, etc. Let us consider one of the examples proposed in the generalization paragraph. If the object the robot grasps is too heavy, we could just add a "1" to the feature vector of latent variables and train the SVM classifier with the new extended vector. Therefore, it could be learned that even when the inferred human's intention is grabbing the object, the robot must never open the gripper if the object is too heavy. Of course, if we consider only one environmental variable, the easiest way to tackle this event would be to add a conditional statement to the control code of the robot. However, if the number of those variables increases, machine learning methods could help. Furthermore, GPLVM allowed us to visualize the distribution of the data we worked with, which could be especially useful if the dataset were enlarged by other people, and we wanted to see how the different datasets related to each other.

#### *4.5. Comparison of Natural and Mechanical Datasets*

In this section, we evaluate and compare the performance of the chosen approach, GPLVM + SVM, using both datasets, the natural and the mechanical. We assumed that the mechanical dataset would show a good performance even with a small sampling window sizes. Given that, we wanted to analyze if the proposed method, for the sampling window of 0.7 s, could work similarly well, not only with the mechanical, but also with the natural dataset. Recall that we chose to use the non-optimized GPLVM inference and 10 latent variables. Although the selected sampling window's size was 0.7, during this section, we tested the approach against the usual five sizes we used along the rest of the document. As was done previously, we used cross-validation without replacement ten times, and the data were randomly split into training (75%) and test (25%) sets.

Figure 12 depicts the F1-score values obtained from the evaluation of GPLVM + SVM against both datasets. This bar diagram shows that indeed, our previous assumption was true. In general, using the mechanical dataset, we obtained better results than with the natural data. Specifically, when the window's size was 0.2 s, the F1-score was even close to 95%. However, we also observed that for the window chosen for our validation with users, 0.7 s, the differences between the performance using any of the datasets were minimal. Therefore, the proposed approach worked quite well even when the dataset contained more natural samples of physical human–robot interaction.

**Figure 12.** Evaluation of the natural (blue) and the mechanical (red) datasets of the approach GPLVM + SVM without optimization and 10 latent variables. The mechanical data need less force information to classify with good quality. However, if the window of the force signal is large enough (more than 0.5 s), the model behaves similarly no matter whether the data are mechanical or natural.

#### **5. Validation: Inferring Operator's Intent in a Realistic Scenario**

To validate the selected approach, GPLVM + SVM, we set up an experiment in which several users individually collaborated with a robotic arm according to the industrial scenario of polishing car emblems. The validation was conducted using fifteen healthy individuals within an age range of 18 to 35. Users were selected among people who had knowledge about the robotics domain and had been in contact with robots before. We did not include people with reduced mobility or any cognitive disability, which could affect the perception of the robot's behavior, endangering the users' integrity. Each of the users received an individual explanation, no more than five minutes, about how they were expected to interact with the robot. This included both general information about the system and particular notions about the expected movements for each of the three classes/intentions. Nevertheless, the users were not allowed to train before the evaluation began, because we wanted to evaluate if there was an adaptation of the user to how the system inferred the different intentions. Users were also informed about their rights, possible risks, and were asked to sign an ethical approval specifically designed for this experiment. Note that we followed CSIC (Spanish National Research Council) ethical procedures and asked for ethical consent from the Human Subject Research Committee of CSIC before the validation was conducted.

Recall that the parameter combination for the chosen approach was: GPLVM without optimization, a sampling window of 0.7 s, and 10 latent variables. In this section, we give the flavor of the validation setup, and we evaluate and discuss the obtained results.

#### *5.1. Setup*

The validation setup was aimed at fulfilling the needs required by a human and a robot to collaborate on an industrial task in which the force exchange is not only present, but fundamental for the accomplishment of the task. Using the force based information, the robot should be able to identify the intent of the operator (Section 3.2) and to adapt its state/behavior to it. In order to provide a bi-directional communication, we equipped the robot with a force sensor, used to measure the interaction from the human to the robot, and an armband made of LEDs through which the robot informed the user of its internal state. The latter allowed us to display different patterns (see Figure 13). The finite state machine of the control of robot during the validation experiment is shown in Algorithm 1.

Recall that this scenario was inspired by a real industrial case in which an operator was meant to inspect and polish car emblems. Please refer to Figure 2a to see the different parts of the robot setup used. We can only show the adapter where the emblem is attached since emblems contain private commercial brand logos and cannot be shown due to confidentiality agreements. Another important aspect related to the setup is how the user is located with respect to the robot. We chose to pose the operator in front of the robot so that the physical interaction was comfortable. During the experiment, the operator will have a rag that would be used to polish. Figure 2b shows an example of the pose of a user while polishing. A video of the validation with users can be found at www.iri.upc.edu/groups/ perception/SIMBIOTS.

#### *5.2. Evaluation*

Each user was asked to perform thirty trials randomly selected from the three operator's intent/actions explained in Section 3.2. We made sure that among the thirty trials, ten corresponded to each of the three classes/intentions. Note that since trials were randomly arranged for each person, there could not be any bias in our evaluation due to the order of the trials. Both the ground truth

and the inferred value were annotated for each user's trial. In this section, we analyze the overall performance of the system (confusion matrix) and the overall adaptation of the users throughout the experimental validation.

A confusion matrix of the performance of the system for each user was computed, then we calculated the final mean confusion matrix shown in Figure 14, which contained the average result for all users. The most obvious observation one can make is that the "move" intent was the easiest to identify. Indeed, the confusion matrix was not symmetric, and this class showed a large percentage of false positives, which was a symptom of a clear bias of the model in favor of this class. This can be better understood by looking at Figure 15. This figure shows the sample distribution in the three-dimensional space defined by the most significant/discriminating latent variables among the ten used. We can observe how the samples from the "move" class fell in the middle of the other two classes, which explains why there were many false positives, shared with the other two classes. However, given the bias in favor of this class and the higher proximity to the "grab" class, this latter was the class with the biggest number of false positives.

As stated before, we also studied if there was an adaptation of the users to the system, which would be observable in the performance of the system along the validation experiments. Recall that users only received a short explanation of the three classes and in which axes they could perform the movements for each action. There was ambiguity among classes, and users had a particular way to move for each action. Because of this, during the first trials, the system's performance was poorer. When we talk of adaptation, we mean that the users understand which movements for each class ensure a better performance of the system. Note that this is possible because users could see the result of the inference.

**Figure 14.** Normalized confusion matrix of the performance of the system during the validation with all users and trials. The matrix is non-symmetric, and the biggest portion of misclassified samples of the classes "grab" and "polish" are inferred as "move", which indicates the existence of some bias in favor of the class "move".

**Figure 15.** Single perspective of the data visualization using the three most discriminating latent variables from the original ten. The distribution of the data in this lower space shows that the samples of the class "move" are rather close to the other two classes, which could be the reason why the model seems to be a bit biased in favor of this class.

We computed the average performance of the system for all the trials and users, and the result showed a positive slope of the trend line for the F1-score (Figure 16). We considered that once the trend line was above 0.8, users had already adapted. In our case, this corresponded to the last five trials of the experiment.

**Figure 16.** Average F1-score of the system for all the users along with the experiment's trials. The positive slope of the trend line for the F1-score is an indicator of the adaptation of the users to the system. Please recall that none of the users followed the same sequential trial set since they were randomly generated.

#### **6. Conclusions**

In this article, we presented our work on inferring operators' intent throughout the execution of an industrial collaborative task in which a robot and an operator exchanged forces while sharing the accomplishment of the task. This work consisted of three major contributions: (a) force based operator's intention inference; (b) force based dataset of physical human–robot interaction; and (c) validation of the whole system in a scenario inspired by a realistic industrial application. In our work, the physical interaction between the robot and the human not only existed, but also played a major role since it was the main source of information for the robot to infer the human's intent. Were humans and robots to collaborate in industrial environments in the factories of the future, the main interaction would be physical. Hence, our work means a step forward to enhance humans' and robots' collaboration in real case studies with more natural and user-friendly interaction. In the future, we will consider exploring other model based representations of the inherent contextual knowledge of collaborative shared tasks, to extend our current system to a wider range of more complicated scenarios.

**Author Contributions:** Data curation, A.O.-A.; software, A.O.-A.; supervision, S.F. and G.A.; Writing—review & editing, A.O.-A., S.F. and G.A.

**Funding:** This work is supported by the Regional Catalan Agency ACCIÓthrough the RIS3CAT2016 project SIMBIOTS(COMRDI16-1-0017) and the Spanish State Research Agency through the María de Maeztu Seal of Excellence to IRI (Institut de Robòtica i Informàtica Industrial)(MDM-2016-0656) and the HuMoUR project TIN2017-90086-R (AEI/FEDER, UE).

**Acknowledgments:** Firstly, this work was possible thanks to the help of the users that were part of the validation phase, so we thank all of them for their contribution. We cannot forget the help provided by Marc Maceira, who was fundamental throughout the dataset gathering and also during the experiment with users.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **References**

c 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## *Article* **Optimized Proportional-Integral-Derivative Controller for Upper Limb Rehabilitation Robot**

**M. Kamran Joyo 1,\*, Yarooq Raza 1, S. Faiz Ahmed 2, M. M. Billah 1, Kushsairy Kadir 1,\*, Kanendra Naidu 1, Athar Ali 3 and Zukhairi Mohd Yusof 1**

Received: 10 June 2019; Accepted: 19 June 2019; Published: 25 July 2019

**Abstract:** This paper proposes a nature inspired, meta-heuristic optimization technique to tune a proportional-integral-derivative (PID) controller for a robotic arm exoskeleton RAX-1. The RAX-1 is a two-degrees-of-freedom (2-DOFs) upper limb rehabilitation robotic system comprising two joints to facilitate shoulder joint movements. The conventional tuning of PID controllers using Ziegler-Nichols produces large overshoots which is not desirable for rehabilitation applications. To address this issue, nature inspired algorithms have recently been proposed to improve the performance of PID controllers. In this study, a 2-DOF PID control system is optimized offline using particle swarm optimization (PSO) and artificial bee colony (ABC). To validate the effectiveness of the proposed ABC-PID method, several simulations were carried out comparing the ABC-PID controller with the PSO-PID and a classical PID controller tuned using the Zeigler-Nichols method. Various investigations, such as determining system performance with respect to maximum overshoot, rise and settling time and using maximum sensitivity function under disturbance, were carried out. The results of the investigations show that the ABC-PID is more robust and outperforms other tuning techniques, and demonstrate the effective response of the proposed technique for a robotic manipulator. Furthermore, the ABC-PID controller is implemented on the hardware setup of RAX-1 and the response during exercise showed minute overshoot with lower rise and settling times compared to PSO and Zeigler-Nichols-based controllers.

**Keywords:** upper limb rehabilitation robot; particle swam optimization (PSO); artificial bee colony (ABC); Ziegler Nichols; Maximum sensitivity

#### **1. Introduction**

The life expectancy of people, and hence, the number of older adults, is increasing. Elderly people are most vulnerable to strokes. Stroke is among the main causes of limb disabilities and can be fatal [1,2]. According to statistics [3], more than 10 million people suffer from a stroke annually. Consequently, patients become dependent on others for their basic life activities. Physical therapy under the supervision of physiotherapists can help stroke patients to restore the functionality of their disabled limbs. However, traditional physical therapy involves manpower and is highly expensive as the number of patients increases. Rehabilitation robots have been introduced recently to reduce the burden on physiotherapists and increase the number of exercises performed during a therapy session. These robots can provide a more reliable service as they do not face monotony and fatigue failures due to the repetitive nature of the exercises. One of the most widely used control mechanisms employed in such robots is a proportional–integral–derivative (PID) controller. This mechanism is famous for its simple structure and robust performance in a wide range of operating conditions [4]. However, it is quite difficult to select and determine the PID controller parameters for the system. The control parameters are tuned to achieve stable closed-loop response of the system and reach desired positions within a certain time. Several approaches have been proposed to optimize the PID controller parameters, such as Zeigler-Nichols, a classical method to tune PID control parameters [5], fuzzy logic controller [6,7] and evolutionary algorithms such as genetic algorithm (GA) [8], and swarm optimization algorithms such as ant colony algorithm (ACO) [9], particle swarm optimization (PSO) [10] and artificial bee colony (ABC) algorithm [11].

The Zeigler-Nichols is a heuristic algorithm based on increasing proportional gain until it reaches the ultimate gain at which output of a control loop system has consistent oscillations. The parameters *Ku* and oscillation period *Tu* are used to parametrize PID gains. Using such a controller produces oscillations which introduce large overshoot, higher rise times and lower settling time in the response.

GA is based on the theory of biological operations and optimizes parameters using mutations and crossover operations [12]. ACO is an evolutionary algorithm inspired by the social behavior of ants searching for food using the shortest path. The success rate of ACO is lower than that of PSO [13]. Fuzzy logic is another technique to optimize the parameters of PID. The rehabilitation robot is controlled by PID and the parameters of PID are optimized using fuzzy logic. Every parameter of the PID controller is characterized by different sets of fuzzy rules, and triangular membership functions are used. Experimental results showed that fuzzy PID offers improved and more effective trajectory tracking performance compared to conventional PID controllers [14,15].

PSO algorithm is an evolutionary computation technique inspired by the social behavior of swarms and fish schools. It was first introduced by Eberhart and Kennedy in 1995. Later Eberhart and Shi introduced inertial weights for PSO to provide the global and local exploration balance. PSO has been found to be robust in optimizing nonlinear problems. The PSO technique evaluates the particle position and velocity, which are updated on every iteration with the aim to reach a global best position of the swarm. Every particle in PSO is treated as a volume-less particle in the search space. PSO requires fewer computational resources than GA, which is prone to premature convergence, while its convergence rate is slower than PSO and ABC [16]. PSO is widely used in many engineering applications because of its advancements. Previously, a PSO based optimized PID controller was used to control a multi fingered robotic hand. Comparison of PSO and conventional PID with fuzzy PID is also presented in the work which shows the better results of PSO-PID [17].

ABC is a heuristic technique which is inspired by the intelligent foraging behavior of honeybees. It was proposed by Karaboga in 2005 [11], and is a simple, robust and population based stochastic optimization algorithm [16,18]. The following three categories of bees make the algorithm unique as compared to other swarm algorithms: employed, onlookers and scout bees [19]. In particular, this meta-heuristic algorithm replicates the foraging behavior of honey bees, where a foraging bee evaluates several characteristics of a food source, such as richness of nectar and the complexity of extracting the energy, and communicates the position of the food source to unemployed bees. The communicated information includes the direction and distance to the food source and its profitability; it is regularly updated so that the best food source can be determined. In a recent study, a comparison reported that by using integral square error (ISE) as the objective function to optimize PID, better performance was obtained using PSO than ABC [14]. However, in terms of transient time response that overshoots in the response of system, the rise and settling times of ABC is better than PSO. Although the rise time of PSO is faster than ABC, in rehabilitation, high speed is not recommended, so the slow rise times of ABC eventually offer the benefit of safe movement of the robot and avoid abrupt movements. Also, the ABC algorithm outputs high quality solutions in terms of fitness value with fewer function evaluations in comparison to PSO. It was found that ABC is more robust than PSO optimized controller [20].

This paper presents a comparative analysis of Zeigler-Nichols, ABC and PSO to determine the tuning parameters of a two-degree-of-freedom (2-DOF) PID controller for robotic arm exoskeleton RAX-1. The objective of applying the mentioned optimization algorithms is to establish the optimal control parameters of the 2-DOF PID by minimizing cost, and to meet the prescribed performance criteria. Four different objective functions, i.e., integral square error (ISE), integral time square error (ITSE), integral absolute error (IAE) and integral time absolute error (ITAE), have been investigated to find the controller with optimal or near optimal load disturbance response subject to robustness and maximum sensitivity constraints. Maximum sensitivity represents the inverse of the minimum distance on the Nyquist plot between critical point and loop transfer function. Such a method for tuning the controller parameters has proven to be effective for robust performance [21]. Furthermore, performance parameters such as overshoot, rise time, settling time and maximum sensitivity are normalized and the least average error (LAE) is evaluated. The optimal solution found for 2-DOF PID for RAX-1 is then implemented with the RAX-1 hardware for trials with three healthy subjects. The rest of the paper is organized as follows. Section 2 explains the controller design, Section 3 presents the simulation results, Section 4 provides the comparative analysis and discussion. Section 5 concludes the paper.

#### **2. Methodology**

RAX-1 is an exoskeleton device meant to be used for rehabilitation of upper limb extremities. Operating alongside the human arm, exoskeleton devices are required to produce movements similar to those performed by the upper limb. There are nine DOFs in the upper limb, excluding finger joints. This study focuses on the glenohumeral joint in the shoulder, which is a complex ball-and-socket joint that enables the shoulder to perform movements in three DOFs. These movements are commonly referred to as shoulder extension/flexion, abduction/adduction and medial/lateral rotation, also known as internal/external rotation. Figure 1 represents the three movements that can be performed with the shoulder joint. The ranges of motion for the shoulder joint movements performed by a healthy subject are listed in Table 1 [22]. These movement protocols are then implemented on RAX-1.

**Figure 1.** (**a**) Shoulder abduction and adduction, (**b**) Shoulder Extension/Flexion, (**c**) Shoulder External and Internal rotation [22].

**Table 1.** Standard ranges of motion of Upper Limb.

#### *2.1. System Design*

The robotic manipulator in the present study comprises two shoulder joints. The DOF of the manipulator can be calculated by the numbers of links and joints. The 3D model of the robot is illustrated in Figure 2.

**Figure 2.** 3D CAD Model of Robot.

A robot in the planner configuration is defined with three parameters (x, y, θ). However, robots are three dimensional in real-world applications; hence, there is a need for six parameters (x, y, z, yaw, pitch, roll) to describe the position and orientation of a robot in space.

Figure 3 represents the direct kinematics of the robotic arm. Every rigid body in a serial chain has a label: Link 1 is the rigid body attached to the shoulder joint 1, Link 2 is the rigid body attached to Link 1 and so on. A joint is present between each link. Hence, Joint 1 attaches Link 1 to Link 0 and Joint 2 attaches Link 2 to Link 1. Frame of reference is numbered according to the respective links they are attached to, e.g. Frame 1 is attached to Link1. Eventually, the aim is to calculate the position of Frame 2 relative to that of Frame 0.

**Figure 3.** (**a**) DH convention for frame assignment, (**b**) Kinematics of the Robotic Arm.

Figure 3a shows a pair of adjacent links which are link (*i* − 1), and link (*i*) with their associated joints, joint (*i* − 1), and joint (*i*). A frame (*i*) is assigned to link (*i*) as follows.

The Denavit-Hartenberg (DH) parameters of a rigid link depends on four geometric parameters (*ai,* α*i, di*, θ*i*) [23]. The four parameters describe any revolute joint as follows:

1. *ai* (Link length) is a distance measured along the *xi* axis from the point of intersection of *xi* axis with z*i* − 1 axis to the origin of frame (*i*).

The 2-DOF upper limb robotic manipulator can be calculated from Figure 3b. The transformation matrix from Frame 0 to the end-effector can be defined as:

$$T\_0^\mathbf{r} = \begin{bmatrix} c\_{12} & c\_{1S2} & -s\_1 & a\_2c\_{12} + a\_1c\_1\\ s\_1c\_2 & -s\_{12} & c\_1 & a\_2s\_1c\_2 + a\_1s\_1\\ -s\_2 & -c\_2 & 0 & a\_2s\_2 + d\_1\\ 0 & 0 & 0 & 1 \end{bmatrix}$$

where *a*1 = 59.4 cm, *a*2 = 105.8 cm and *d*1 = 20 cm. The transformations can be used to determine kinematic measurements of the joints. For any joint angle, the position of the end effector can be derived from the transformation matrix.

#### *2.2. Dynamic Model*

In this study, Euler-Lagrangian approach was applied to calculate the dynamics of the robot manipulator. This approach uses the joint velocities and position to determine the kinetic and potential energies of a system. It generalizes Newtonian mechanics for systems that are subject to a specific class of constraints. These constraints are often expressed in terms of the position or variables describing the system in question.

The Lagrangian equation of motion defined in (1) is written as:

$$\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}\_j}\right) - \frac{\partial L}{\partial q\_j} = \tau\_j \tag{1}$$

where τ*j* denotes the required torque, L = K − P (Kinetic and potential energies) is the Lagrangian and *qj* is the generalized coordinate of the *j*th joint of robot.

The inertial matrix *D*(*q*) can be determined as follows:

$$D(q) = m\_1 I\_{\rm rc1}^T I\_{\rm rc1} + m\_2 I\_{\rm rc2}^T I\_{\rm rc2} + \begin{bmatrix} I\_1 + I\_2 & 0 \\ 0 & I\_2 \end{bmatrix} \tag{2}$$

where *m*1 = 1.5 kg, *m*2 = 0.5 kg. Simplifying (2), one can obtain the following (3).

$$\begin{aligned} d\_{11} &= m\_1 a\_{c1}^2 + 2m\_2 a\_1 a\_{c2} c\_2 - 2a\_{c2} d\_1 m\_2 s\_2 + a\_1^2 m\_2 + a\_{c2}^2 m\_2 + d\_1^2 m\_2 + l\_1 + l\_2 \\ d\_{12} &= m\_2 a\_1 a\_{c2} c\_2 - m\_2 d\_1 a\_{c2} s\_2 + m\_2 a\_{c2}^2 \\ d\_{21} &= m\_2 a\_1 a\_{c2} c\_2 - m\_2 d\_1 a\_{c2} s\_2 + m\_2 a\_{c2}^2 \\ d\_{22} &= m\_2 a\_{c2}^2 + l\_2 \end{aligned} \tag{3}$$

The correction term Christoffel symbols ensures that when the derivatives of the vector field lying in a tangent plane of the configuration manifold are computed, they stay in the same tangent space. The Christoffel symbols in (4) are defined as

$$\begin{aligned} c\_{111} &= \frac{\partial d\_{11}}{\partial q\_1} = 0\\ c\_{121} = c\_{121} = \frac{1}{2} \frac{\partial d\_{11}}{\partial q\_2} = -m\_2 a\_1 a\_{c2} c\_2 - m\_2 d\_1 a\_{c2} c\_2 = h\\ c\_{221} &= \frac{\partial d\_{12}}{\partial q\_2} - \frac{1}{2} \frac{\partial d\_{22}}{\partial q\_1} = h\\ c\_{112} &= \frac{\partial d\_{21}}{\partial q\_1} - \frac{1}{2} \frac{\partial d\_{11}}{\partial q\_2} = -h\\ c\_{122} &= c\_{212} = \frac{1}{2} \frac{\partial d\_{22}}{\partial q\_1} = 0\\ c\_{222} &= \frac{1}{2} \frac{\partial d\_{22}}{\partial q\_2} = 0 \end{aligned} \tag{4}$$

The potential energy of each joint *Pi*, is the product of the mass of that link *mi*, position vector to the centre of mass *rci* and acceleration due to gravity *g*:

$$P\_{\mathbf{i}} = m\_{i} \mathbf{g} \tau\_{\mathbf{ci}\prime} \mathbf{i} = \mathbf{1}, \mathbf{2} \tag{5}$$

The term φ*k* is a function of generalized coordinates that does not depend on their derivatives. It is given by the partial derivative of potential energy of the system with respect to the generalized coordinates as follows:

$$
\phi\_k = \frac{\partial P}{\partial q\_k} \tag{6}
$$

Finally, the dynamic equations of the system after substituting various quantities and omitting zero can be expressed as

$$\begin{pmatrix} d\_{11}\ddot{q}\_1 + d\_{12}\ddot{q}\_2 + c\_{121}\dot{q}\_1\dot{q}\_2 + c\_{211}\dot{q}\_2\dot{q}\_1 + c\_{211}\dot{q}\_2^2 + \phi\_1 = \tau\_1\\ d\_{21}\ddot{q}\_1 + d\_{22}\ddot{q}\_2 + c\_{122}\dot{q}\_1^2 + \phi\_2 = \tau\_2 \end{pmatrix} \tag{7}$$

which, in general can be written in matrix form as:

$$D(q)\ddot{q} + \mathcal{C}(q, \dot{q})\dot{q} + \mathcal{g}(q) = \tau \tag{8}$$

Here, *D*(*q*) denotes the inertia matrix of the system and *C q*, . *q* gives the Christoffel symbols and *g*(*q*) is actually φ*k* which is determined by taking partial derivative of potential energy with generalized coordinates. τ is a 2 × 1 matrix representing the generalized active forces.

#### *2.3. Linearized Model*

The linearized state space model for robot exoskeleton (RAX-1) is expressed as follows.

$$A = \begin{bmatrix} 0 & -5.5510\epsilon - 13 & 0 & 0\\ 1 & 0 & 0 & 0\\ 0 & 0 & 0 & -25.8600\\ 0 & 0 & 1 & 0 \end{bmatrix}, B = \begin{bmatrix} 1 & 0\\ 0 & 0\\ 0 & 1\\ 0 & 0 \end{bmatrix} \tag{9}$$

$$C = \begin{bmatrix} 0 & 31.66 & 0 & 0\\ 0 & 0 & 0 & 6.3760 \end{bmatrix}, D = \begin{bmatrix} 0 & 0\\ 0 & 0\\ 0 & 0 \end{bmatrix}$$

#### *2.4. Motor Model*

The robot manipulator requires actuators to provide the desired amount of torque at the joints. The actuators convert electrical energy into rotational mechanical energy. DC motors are widely used in robotics as actuators due to their high torque, speed controllability and portability [24]. The internal model of DC motor is illustrated in Figure 4 and can be expressed as follows.

$$\frac{\theta(\mathbf{s})}{\mathbf{E\_{a}(s)}} = \frac{K\_{\text{r}}}{(L\_{a}\mathbf{s} + R\_{a})(Js + B\_{m}) + K\_{b}K\_{\text{r}}} \approx \frac{K\_{\text{r}}}{R\_{a}(Js + B\_{m}) + K\_{b}K\_{\text{r}}} \tag{10}$$

**Figure 4.** Internal model of DC motor.

Here,'*J*' is the motor inertia, '*Bm*' motor damper, '*K*τ' is the motor constant, '*Kb*' is the proportionality constant between angular velocity of the motor and back emf, whereas '*La*' and '*Ra*' inductance and resistance of the armature. If *La Ra* then, an approximated transfer function of motor is obtained by setting *La* = 0. This converts motor model from a second order system to 1st order system. DC motors with harmonic gears are used in this system. Table 2 lists the motor parameters.

**Table 2.** Motor Parameters.

#### **3. The Exoskeleton Platform**

In this section, the hardware design for the upper limb extremity rehabilitation is described. The platform consists of the design and manufacturing of the mechanical structure, actuators and sensors with hardware implementation of the control algorithm.

#### *3.1. Mechanical Structure*

The 2-DOF mechanical platform is built using aluminum grade 6061 alloy and weighs approximately 15 kg. The structure is specifically designed to focus on specific exercises for rehabilitation of the shoulder joint (Figure 5).

The upper extremity exoskeleton device consists mainly of a support frame, height adjustment mechanism and shoulder actuation mechanism. The support frame is attached to wheels enabling the platform to be remote. The manually controlled height regulation mechanism is attached to the support frame. The actuation mechanism is attached to the height regulation mechanism; it adjusts the mechanical frame fixed to human arm to match requirements to the subject height. The human arm is fixed to the exoskeleton with soft wraps both on the forearm and bicep. Other relevant adjustments are made during gait training. Joints at the wrist and elbow are passive and their orientations are fixed.

**Figure 5.** RAX-1 Mechanical design.

#### *3.2. Actuators and Sensors*

The two-revolute joint system is equipped with the MAXON EC-90 motor (Maxon Motor, Sachseln, Switzerland) attached to the gear. A brushless flat motor has the maximum angular velocity of 2590 rpm and power rating of 90W which can generate maximum torque of 444 mNm. It is equipped with three Hall effect sensors to measure current and velocity and an internal encoder generating 2040 pulses per rotation. The reducers (harmonic drives) combined with flat motor have speed ratio of 160:1 for the first shoulder joint responsible for the internal/ external movement, while the ratio is 150:1 for the other joint responsible for the extension/ flexion movement. The detailed specification of other motor parameters and gear ratio is provided in Table 2. A force sensor is attached to the wrist handle of the robot, from which external disturbance exerted by the subject is measured while the exoskeleton is working in passive mode. The electrical setup of the system involves an ESCON 50/5 module which acts like a closed-loop speed or current controller for the motor.

#### *3.3. Control Implementation*

In the control system, a host computer is used as a graphical user interface. The software is written in visual C#; it allows the user to select one of the possible working modes of the exoskeleton (passive, active or semi-active). The software enables uploading and downloading subject's history over a cloud specifically designed for this device. The user can also define the range of motion at which the exoskeleton should work for several repetitions.

 CC3200 from Texas Instrument is used as a peripheral device that controls the angular movement. The master is instructed by the user to select an exercise and the number repetitions, as well as set an angular movement via an interface. This information is then communicated to the slaves for further implementation of the task [25] as depicted in Figure 6a,b. The overall system block diagram is illustrated in Figure 6c.

**(b)**

**Figure 6.** (**a**) HMI displaying rehabilitation training modes, (**b**) HMI displaying selection for shoulder exercise, (**c**) System Block Diagram.

Figure 7 represents the controller card used for RAX-1; it consists of master-slave network and drivers used for motors.

**Figure 7.** Controller Board.

#### **4. The Control Algorithm**

#### *4.1. Proportional Integrator Derivative (PID) Controller*

One of the most widely used controllers is the PID controller, which has a very simple structure and is robust in under wide range of operating conditions. The PID error signal, which is the difference between setpoint and measured variable, is calculated and fed back into the system continuously to change the proportional, integral and derivative gains accordingly [26].

PID is one of the most used control algorithms in industrial control systems. The response of a system is categorized by the rise time, overshoot, settling time and steady state error. In this study, a 2-DOF PID controller is used, and is represented mathematically according to Equation (11). The controller is capable of rejecting disturbances without significant increase of overshoot in setpoint tracking. It includes setpoint weighting on the proportional and derivative terms. A typical 2-DOF PID is composed of feed-forward and feedback compensators. The feed-forward compensator consists of a PD component while the feedback compensator includes PID component as shown in Figure 8.

$$u = K\_p(b.r - y) + K\_i(r - y)s^{-1} + K\_d \frac{N}{1 + Ns^{-1}}(c.r - y) \tag{11}$$

where *u* denotes the input given to the plant or system, while *Kp*, *Kd***,** *Ki*, denotes the proportional, derivative and integral gains. In this study, 2-DOF PID in MATLAB is used, which has three more parameters to tune; *b*, *c* and *N*, where; *b* and *c* denote the setpoint weights, while *N* denotes a filter coefficient.

**Figure 8.** 2-DOF PID block diagram.

#### *4.2. Particle Swam Optimization (PSO)*

PSO is an optimization technique inspired by the social behavior found in nature, such as flocking of birds and fish schooling. In the PSO search space, each solution acts like a flying bird in the search of food, known as a particle. PSO works based on the social behavior of particles in a swarm. This algorithm locates and finds the global best solution by adjusting an objective function. At the end of the process, best solution based on objective function is found for 2-dof pid controller parameters.

First, PSO chooses random solutions to initialize the population. Then, it updates its performance to obtain optimum value. Every particle is characterized by its position and velocity in the swarm. The velocity of a moving particle depends upon the change in position or direction [27]. Each particle updates its new position based on two components, pbest and gbest, where pbest is the best position attained by a particle, while gbest is the global best position of the entire swarm. The velocity and position of the particle can be expressed according to Equations (10) and (11), respectively.

$$w\_{ij}(t+1) = wv\_{ij}(t) + r\_1c\_1(p\_{ij}(t) - x\_{ij}(t)) + r\_2c\_2(g\_j(t) - x\_{ij}(t))\tag{12}$$

$$\mathbf{x}\_{i\rangle}(t+1) = \mathbf{x}\_{i\rangle}(t) + \upsilon\_{i\rangle}(t+1) \tag{13}$$

where; *vij*(*t*) denotes the particle velocity, *xij*(*t*) denotes the particle position, *vij*(*t* + 1) denotes the particle updated velocity, *xij*(*t* + 1) denotes the particle updated position; *pij*(*t*) denotes the particle best position; *gj*(*t*) denotes the global best position of the swarm; *w* ,(*w* = *wmax* − *wmin*) denotes the inertia term; *r*1 and *r*2 are two uniformly distributed random numbers ranging from 0 to 1 and *c*1 and *c*2 are the acceleration coefficients. The important steps of the PSO are summarized in Figure 9.

According to the PSO algorithm, the swarm size, position, velocity and the constants *w*, *c*1 and *c*2 are initialized first. Then, the fitness value of each particle is calculated, *Pbest* and *gbest* are defined and the position and velocity of each particle are updated. The algorithm stops when the stopping criterion is met. The optimal solution is chosen according to the latest *gbest*. The PSO parameters and their values used in our study are provided in Table 3.

**Table 3.** PSO Parameters.

#### *4.3. Artificial Bee Colony (ABC)*

ABC is an optimization technique based on the smart behavior of honey bees [16], which are famous for their intelligence and ability to perform complex tasks such as collecting nectar and building nests with a high degree precision [28]. Information about the quality of a food source is communicated within the colony by a particular dance language. The precision of the foraging range of honeybees allows an efficient exploitation of food sources and concentration of foraging on the best patches. There are two main concepts that describe swarm intelligence, namely self-organization and labor division [29]. The self-organizing behavior represents the complex collective behavior that rises from the local interaction between the agents showing a simple self-directed behavior. The mechanism of labor division assigns specific tasks to the agents performing simultaneous activities, which results in a more efficient and time-saving performance.

**Figure 9.** PSO Optimization Flow.

There are three categories of artificial bees in ABC algorithm: employed, scout and onlooker bees. The employed bees go to the food source that has already been visited by them, while unemployed bees look for further sources of food. The number of employed bees is equal to that of food sources. Searching for new sources is performed by the scout bees, whereas the onlooker bees wait for the information about the discovered food sources provided by employed bees via their waggle dance. If the position of a food source does not improve within a number of attempts known as limits, then the employed bees become scout bees. In this manner, the exploitation process is performed by employed and onlooker bees, whereas the scout bees explore the existing solutions. The details of different ABC phases are described in the following subsections.

*a. Initialization Phase*

Random initialization of the locations of food sources is performed according to the following equation:

$$\mathbf{x}\_{i\bar{j}} = \mathbf{x}\_{\bar{j}}^{\text{max}} + rand(0, 1) \mathbf{x}\_{\bar{j}}^{\text{max}} - \mathbf{x}\_{\bar{j}}^{\text{min}}); \ i = 1, \ldots, \text{SN}, \ j = 1, \ldots, D \tag{14}$$

where *SN* is the number of food sources taken as half of the bee colony, *D* is the dimension of the problem, *xij* denotes the *i th* employed bee on *j th* dimension, wjile *xmax j* and *xmin j* denotes its upper and lower bounds.

#### *b. Employed Bee Phase*

Every other employed bee is allocated with the food source for further exploitation. Equation (11) represents the process of generating a food source.

$$w\_{i\bar{j}} = \mathfrak{x}\_{i\bar{j}} + \phi(\mathfrak{x}\_{i\bar{j}} - \mathfrak{x}\_{k\bar{j}}) \tag{15}$$

where φ = *a* × *rand*(0, 1) is a random variable denoting the acceleration coefficients ranging in the interval [−1,1] and *vij* is the new solution or a food source. The fitness *fiti* of the new food source is calculated according to the following equation:

$$fit\_i = \begin{cases} \frac{1}{1+f\_i}, & f\_i \ge 0\\ 1+abs(f\_i), & f\_i < 0 \end{cases} \tag{16}$$

where, *fi* is the objective function of each food source. A selection is made between the original and new food sources to choose the better one by keeping the fitness value in accordance.

*c. Probabilistic Selection Phase*

An onlooker bee selects a food source with a certain probability calculates as

$$p\_i = \frac{fit\_i}{\sum\_{j=1}^{N} fit\_j} \tag{17}$$

where *pi* denotes the probability of selecting the *i*th solution.

#### *d. Onlooker Bee Phase*

The employed bees share the information about food sources with the onlooker bees, who select a food source to exploit better solutions according to its selection probability. The fitness values of each exploited food sources is calculated. A greedy selection between original and new food sources is made similar to the employed bees phase.

#### *e. Scout Bee Phase*

If a food source does not yield better results within the limits *L*, where *L* = 0.6 × (*Number o f optimization parameters*) × (*colony size*), then this food source is abandoned and the bee associated with it becomes a scout bee. In this case, a new source of food is randomly generated according to Equation (15). All phases will continue until the termination criterion is met. The output is the best food source solution. Figure 10 shows the flowchart of the ABC algorithm which depicts the process of 2-dof pid controller optimization.

According to the ABC algorithm, the colony size, position and the constants *L* and *a* are initialized first. Then, the fitness value is calculated, and the best food source is defined. The algorithm stops when the stopping criterion is met. The optimal solution is chosen according to the latest *gbest*. The ABC parameters and their values used in our study are provided in Table 4.

**Figure 10.** ABC Optimization Flow.

**Table 4.** ABC Parameters.

#### **5. Results and Discussions**

Based on the simulation, the system is divided into two linearized sub systems representing Joint 1 and Joint 2 [30]. Independent controls for both joints with saturation limits are implemented. A step input is given to the model of the robot and the response is observed. A schematic of the proposed controller's tuning method based on PSO and ABC is shown in Figure 11.

**Figure 11.** General Structure of the proposed control System Diagram.

For this research, a comparative study was carried out with four different cost functions to gauge the appropriate objective function for this study. A well-chosen objective function leads to better performance of the system and meets control design expectations. These objective functions include the integral squared error (ISE), integral time squared error (ITSE), integral absolute error (IAE) and integral time absolute error (ITAE), and are defined as follows:

$$ISE = \int e^2 dt\tag{18}$$

$$IAE = \int |c|dt\tag{19}$$

$$ITEE = \int t.c^2 dt\tag{20}$$

$$ITAE = \int t. |e|dt\tag{21}$$

#### *5.1. Robustness Consideration*

Focus on the tuning procedure requires some robustness considerations in the design. This is achieved by using the maximum sensitivity function as a measure of robustness and is given by

$$M\_{\\$} = \max\_{\omega} \left| \mathbb{S}(j\omega) \right| = \max\_{\omega} \frac{1}{\left| 1 - \mathbb{C}\_{y}(j\omega)P(j\omega) \right|} \tag{22}$$

where *S*(*j*ω) ≤ *Ms*. The sensitivity function shows the effect of feedback on the output. Disturbances are attenuated if *S*(*j*ω) < 1 and are amplified if *S*(*j*ω) > 1. The robustness of the closed loop increases with the decrease in *Ms*. The values for *Ms* ranging from 1.2 to 2.0 provides reasonable robustness [27].

#### *5.2. Simulation Setup*

Before implementing ABC-PID [11] on hardware, simulations are carried out to validate and verify the control algorithm. In this section, a comparative analysis of the PID controller optimized with ABC, PSO, and conventional tuning method based on Zeigler-Nichols is presented. The parametric bounds

are provided in Table 5. The Zeigler-Nichols PID parameters and PID parameters tuned using ABC and PSO used for simulation are provided in Tables 6 and 7. The simulations presented here are for internal/external rotation, extension/flexion and abduction/adduction of the shoulder joint.

**Table 5.** Parametric Constraints used for PSO and ABC.

**Table 6.** Optimized PID Parameters for Joint 1.

**Table 7.** Optimized PID Parameters for Joint 2.

To evaluate the fitness of the objective function, normalization of the objective function is carried out, which scales objective function within a specified range. The following normalization function is used [31].

$$f\text{it}'\_{i} = \frac{f\text{it}\_{i} - \min(f\text{it}\_{\text{overall}}) \times \delta}{\max(f\text{it}\_{\text{overall}}) - \min(f\text{it}\_{\text{overall}}) \times \delta} \tag{23}$$

where *fiti* represents the objective to be normalized and *fitoverall* represents the overall fitness. δ is kept 0.99 to avoid any zeros during normalization. Furthermore, normalized average has been evaluated to determine the significance of the objective function.

Table 8 shows that IAE and ITAE used as objective functions produce minimal overshoots and suitable rise and settling times for both PSO-PID and ABC-PID. When using ISE and ITSE, the rise time is small, while larger overshoots are detected. However, it is obvious that the performance parameters of ABC-PID for Joints 1 and 2 are much better in terms of the six performance parameters mentioned above compared to that of PSO-PID or Zeigler-Nichols.

The results plotted in Figure 12 show the response for Joint 1 for PID controller tuned using the Zeigler-Nichols and optimal parameters found with PSO and ABC for all four-objective functions. The figure shows that the ZN response is quicker with a higher rise time, higher settling time and larger overshoot. The results also show a very low percentage of overshoot in the response with a very low steady state error for all four objective functions. However, the optimal response of the system is found while using IAE as objective function.

**Figure 12.** Step response of Joint 1 using ISE, IAE, ITSE and ITAE.

Figure 13 shows the step response for Joint 2. It is evident from the figure that PID tuned Zeigler-Nichols produces large overshoot than the PID optimized with ABC or PSO. However, the overshoot of ABC is 4.4% which is smaller as compared to 22% overshoot of PSO when using IAE. It was found that PID-ABC has the minimum average of the normalized objective function for Joint 1 and Joint 2. Hence, the ABC-optimized PID controller is found to be robust and stable for practical implementation on a robotic arm manipulator. The performance and robustness obtained by calculating the system percent overshoot, rise time, settling time, cost and sensitivity is presented in Table 8.

**Figure 13.** Step response of Joint 2 using ISE, IAE, ITSE and ITAE.

Furthermore, an investigation on closed-loop stability analysis is performed by Nyquist plot of the controllers tuned with ABC using IAE for the given systems. The plot for Joint 1 and Joint 2 are shown in Figure 14. It indicates that the both systems are asymptotically stable as no poles lie in the right half of the plane.

**Figure 14.** Nyquist plot for closed loop system of joint 1 and joint 2.

#### *5.3. Experimental Evaluation for RAX-1*

A preliminary experiment was conducted to test the efficacy and performance of the exoskeleton controlled using the optimized PID controller. Three healthy male subjects participated in the experiment and their properties are listed in Table 9. Subjects were bound with soft wraps and their preliminary tests were performed before and after the exercise. The tests included measuring the body temperature, heart rate and oxygen levels. The evaluation procedure was approved by the Ethics committee of the University of Kuala Lumpur (UniKL), Kuala Lumpur, Malaysia. All subjects were volunteers who signed consent forms before the experiment. The procedure was completed in the presence of physiotherapists from the Royal College of Medicine Perak, Malaysia. The mechanical structure used for this experiment was RAX-1 illustrated in Figure 5.

In this experiment, the upper extremity exoskeleton, which is a robot in charge of the rehabilitation protocol, provides passive assistance for the subjects. The two actuators follow the specific trajectories of a pre-defined range of motions. The followed trajectory and joint angles can be measured from the encoders. Figure 15 shows a subject performing the shoulder exercises with the assistance of exoskeleton device. The angular rotation of the joint can be set via the interface, and its current position can be observed on the screen.

A ramp/ sinusoidal repetitive input is provided to the shoulder joint to perform three different exercises, namely shoulder extension/flexion, internal/external rotation and abduction/adduction (Table 1).

For this experiment, a combined shoulder external/internal rotation was performed by the first subject; the upper bound movement during external rotation was limited to 90◦ and that during internal rotation was limited to 0◦. Figure 16a illustrates the motion tracking of the robotic arm where a minute overshoot was observed in reaching the target reference. The graph also shows the error representation between the reference and actual response and speed of the motor in terms of rpm. During the experiment, each subject went through the same gait pattern, i.e. the movement was set to a frequency of 0.25 Hz for internal/external rotation.

**Figure 15.** A subject performing exercise with assistance of exoskeleton device.

**Figure 16.** (**a**) Shoulder Internal/External Rotation, (**b**) Current driven from the motor and Disturbance applied by first subject.

Figure 16b shows the current required by the motor to move the shoulder to a desired angle. It also represents the amount of external force exerted by the patient on the system. The external force acts as an external disturbance to the system and its effect can be seen in the Figure 16b. As the external force to resist the movement increases, the motor current drastically increases to overcome the effect of disturbance. The maximum current drawn by the motor is 5.27 A when the maximum external resisting force of 68 N is applied to the joint.

The second exercise, namely shoulder abduction/adduction, was performed by the second subject; the bounds are set from 0◦ to 90◦. Figure 17a represents the response of the system during shoulder abduction and adduction. It also represents the difference between the reference signal and response, which demonstrates a minute overshoot in the response. The gait pattern for this exercise is similar to that for the previous exercise, where the movement is set to a frequency of 0.25 Hz.

**Figure 17.** (**a**) Shoulder Abduction/ Adduction, (**b**) Current driven from the motor and Disturbance applied by second subject.

Figure 17b represents current drawn by the motor to achieve the target angle and the external force applied by the subject. When external resistance was exerted on the system by the subject, an unusual activity in the current was detected to overcome the external disturbance. A maximum current of 5.14 A was observed with 50N of applied force. The impact of this resistance can also be observed in Figure 17a, where disruption occurs when external force is applied while achieving the reference.

The third exercise, namely, shoulder extension/flexion, was performed by the third subject; the range of motion was fixed to oscillate between 0◦ to 90◦. In this experiment a pulsating input was fed as a reference (see Figure 18).

**Figure 18.** (**a**) Shoulder Extension/Flexion, (**b**) Current driven from the motor and Disturbance applied by third subject.

Figure 18a shows the motion tracking of the shoulder joint while performing extension/flexion. It also shows the error between reference and actual angle with the speed of the motor. In this exercise, each subject went through same gait pattern i.e. the exoskeleton is set to move with the frequency of 0.25 Hz for extension/flexion. Figure 18b shows the current driven by the motor to perform the exercise and force exerted by the patient to resist the movement. It can be seen that the motor draws more current than usual where the external force is applied; its impact on the movement can also be observed in Figure 18a. It is evident from the graph that a small amount of overshoot with no steady state was observed. The maximum force applied by the subject was recorded at 55N, while 4.5A of current was drawn by the motor.

#### *5.4. Comparison of the PID Controllers Optimized with ABC and the Zeigler-Nichols Method*

An experiment was performed to compare PID tuned controllers tuned with ABC and the Zeigler-Nichols method under the effect of disturbance. The parameters set for both techniques are similar to those defined in the simulation setup. The experiment was performed with the extension/flexion movement and the range of motion for both techniques was the same. The response of the system using ABC optimization and the Zeigler-Nichols method is shown in Figure 19.

**Figure 19.** (**a**) Response of the system with ABC optimized PID and tuned using Zeigler-Nichols, (**b**) Error of the system with ABC optimized PID and tuned using Zeigler-Nichols.

It can be seen from Figure 19 that PID controller tuned with the Ziegler-Nichols has a low settling time and quick response. However, the response of the system is not smooth and has a steady state error. At the same time, ABC-optimized PID response is slower due to a greater settling time. However, it has a smooth response with virtually no steady-state error. The angular rotation of the arm is set to move from 0◦ to 90◦. Figure 19a illustrates the response of the system tuned with ABC and the Zeigler-Nichols method, while the error signal is shown in Figure 19b. It is evident from the graph that the rise time of ZN-PID is lower than ABC-PID. The overshoot and steady state produced by ZN-PID are also larger than those of ABC-PID. Zeigler-Nichols based PID produces an angular drift of 3.64◦ while it is 0.63◦ for ABC-PID. A slower response is favorable for the rehabilitation system used in this study. The presence of large overshoot and steady state in the system is dangerous, as it may cause dislocation or fracture of the arm. Hence, it can be concluded that PID controller works better when optimized with ABC compared to PSO or Zeigler-Nichols method as ABC-PID produces very low overshoot, slower rise time and no steady state.

#### **6. Conclusions**

Rehabilitation robotics have been studied for decades, but few researchers have ever considered using optimization techniques for gait training. This paper presents PSO- and ABC-based tuning techniques for a 2-DOF PID controller used in the robot controlling trajectories of different exercise movements performed by the shoulder joints, namely internal/external rotation, abduction/adduction, and extension/flexion. RAX-1 was used as a mechanical experimental platform. The control parameters of the PID controller were tuned using the ABC and PSO algorithms as well as the Zeigler-Nichols method. The simulation results demonstrated better feasibility of the proposed controller in terms of robustness. An ABC-optimized PID controller was also implemented into the hardware for three subjects, with each performing different exercises. It was necessary for the robot to perform steady motion with no steady state error during the process of rehabilitation, as any abrupt movement could dislocate the shoulder joint. The hardware results showed that the controller could trace the desired trajectory with a very minute overshoot in the response and significantly low response times, which are desirable in rehabilitation. Finally, this study compared the performance of PID controllers optimized with ABC and the Zeigler-Nichols method, respectively. The controller tuned using the Zeigler-Nichols method demonstrated a decent rise time but large overshoot in the response, which is dangerous for rehabilitation applications. However, the hardware response of the system had less overshoot and no steady-state error when the ABC optimizer was used to tune the PID parameters. In summary, this study focused on finding optimal parameters of the PID controller used in an upper limb rehabilitation robotic system. In the future, we plan to broaden the spectrum of this study by incorporating various protocols related to the elbow and wrist rehabilitation with an updated mechanical structure. Analysis with other notable optimization techniques such as firefly and ant colony algorithms will be further investigated to compare their parameters and validate their capabilities in rehabilitation applications.

**Author Contributions:** K.N. and S.F.A. conceived the presented idea to use optimization of controller parameters for rehabilitation robotic system. M.K.J. and Y.R. implemented the idea and drafted this manuscript. M.M.B. and A.A. massively contributed in developing the mathematical model for robot exoskeleton. The project has been supervised by K.K. and Z.M.Y.

**Funding:** This research has been generously supported by a research grant by the Ministry of Science, Technology and Innovation (MOSTI) under the Program Flagship DSTIN (FP0514D003-2) for the development of a new technology identified as "Building Our Robotic Competitiveness in Medical Healthcare: Development of Robots for Assisted Recovery and Rehabilitation".

**Acknowledgments:** We would like to pay our gratitude to Universiti Kuala Lumpur for providing great research environment. The authors are also grateful to Technology Park Malaysia for their assistance in developing the mechanical structure for robot exoskeleton.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **References**

© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## *Article* **RTPO: A Domain Knowledge Base for Robot Task Planning**

#### **Xiaolei Sun, Yu Zhang \* and Jing Chen**

College of Intelligence Science and Technology, National University of Defense Technology, Changsha 410073, China<EMAIL_ADDRESS>(X.S<EMAIL_ADDRESS>(J.C.)

**\*** Correspondence<EMAIL_ADDRESS>Tel.: +86-139-7585-6486

Received: 21 August 2019; Accepted: 26 September 2019; Published: 1 October 2019

**Abstract:** Knowledge can enhance the intelligence of robots' high-level decision-making. However, there is no specific domain knowledge base for robot task planning in this field. Aiming to represent the knowledge in robot task planning, the Robot Task Planning Ontology (RTPO) is first designed and implemented in this work, so that robots can understand and know how to carry out task planning to reach the goal state. In this paper, the RTPO is divided into three parts: task ontology, environment ontology, and robot ontology, followed by a detailed description of these three types of knowledge, respectively. The OWL (Web Ontology Language) is adopted to represent the knowledge in robot task planning. Then, the paper proposes a method to evaluate the scalability and responsiveness of RTPO. Finally, the corresponding task planning algorithm is designed based on RTPO, and then the paper conducts experiments on the basis of the real robot TurtleBot3 to verify the usability of RTPO. The experimental results demonstrate that RTPO has good performance in scalability and responsiveness, and the robot can achieve given high-level tasks based on RTPO.

**Keywords:** ontology; robot task planning; knowledge base; knowledge representation

#### **1. Introduction**

Nowadays, artificial intelligence (AI) has become a growing field in which many theory studies and practical applications are enjoying a boom. Deep learning, a typical approach to AI, has propelled its entry into a new stage of development [1]. In the field of robotics, AI technology represented by deep learning has also grown to be extensive and vital in several applications [2,3]. However, the deep learning model, a kind of end-to-end learning, helps to achieve uninterpretable and opaque results, which limits its application in some areas requiring knowledge reasoning. For example, in the field of robot combat task planning, the plans need to be interpretable, so that the operational commander can evaluate the advantages and disadvantages of the plans.

Since the 1970s, AI researchers have gradually realized that symbolic knowledge representation is exerting a key role in more powerful AI systems, believing that knowledge and reasoning is the core of AI. From then on, ontology has been developing robustly as a form of knowledge base. It can represent and understand the sophisticated world. Until now, it has been widely employed in various fields such as AI [4,5], Semantic Web [6,7], Information Science [8], etc. Ontology-based task planning is essentially a series of relevant queries and reasoning on ontology knowledge [9]. Made up of a large number of individuals, concepts, and their semantic relations, it can first interpret the elements of the query and reasoning path of the task planning process on ontology knowledge [10]. Similar to a human's thinking mode, a robot can also utilize knowledge and knowledge reasoning to realize smart high-level decision-making.

The challenge in building RTPO is how to efficiently and reasonably represent the intricate task knowledge. There is a need for thorough consideration of temporal and spatial information, as well as continuous and discrete information. Furthermore, it needs to have good performance in scalability and responsiveness, so as to guarantee the usability in the task planning algorithm.

The world knowledge, which is categorized into static knowledge, can be easily and feasibly exhibited, as is said by the previous researches on robot task planning. In this regard, as a result of the feasibility of representing world knowledge, the problem file of the task planner can be obtained from the world knowledge efficiently [11,12]. However, comparatively, the causal knowledge [13], is more represented in the formal language, like the PDDL (Planning Domain Definition Language) [14], HTN (Hierarchical Task Network) [15], and so on. Correspondingly, as is shown in Figure 1, the domain file of the task planner is inclined to come from the causal knowledge manually, which makes it comparatively complex and lacking universality in large-scale applications. Consequently, the paper proposed a task planning algorithm based on the built RTPO, avoiding the complex process of manual generation of domain file.

**Figure 1.** Knowledge representation in robot task planning.

The representations of high-level tasks and atomic actions in RTPO are independent from each other. Then, task planning is realized by matching the execution preconditions of atomic actions and their effects on the environment from the initial state to the goal state. In this way, with the changing of input tasks, the task planning module can also be carried out in accordance with the present atomic action resources. The plans generated by the task planning algorithm will add and update to RTPO, and will improve the efficiency of task planning if the same task needs to be planned next time.

The research work mainly involves two aspects. Firstly, an ontology knowledge base is built for robot task planning and a method to evaluate the knowledge base is proposed. Secondly, an experiment with an indoor study case is carried out to verify the usability of RTPO and the flexibility of the proposed task planning algorithm. During the real robot experiments, we used the ROS (Robot Operating System) [16] under Ubuntu as the software system, and adopted TurtleBot3 as the hardware platform. The experimental results demonstrate that RTPO is of good usability and the task planning algorithm harbors flexibility to address the unexpected events.

There are some typical study works and applications on robot knowledge base. KnowRob [17–19], an integrated knowledge management system for autonomous robots, aims at the construction of an indoor service robot knowledge base. It is composed of ontology, entities, and extensible reasoning engine in OWL language. However, KnowRob holds the limitation of having less abundant knowledge. RoboEarth [20–22], based on KnowRob, has already given definition to sub-actions of specific tasks, along with the definition of the temporal and spatial constraints among different sub-actions. That is, when a high-level task needs to be planned, the user should request the high-level task to generate its corresponding plans. As the task planning methods have to rely on specific high-level task instructions, it is possible that task planning will fail to input other different task instructions.

Besides this, ORO (OpenRobots Common-Sense ontology) [23,24] is built in OWL and stored in the OpenJena ontology management library. The knowledge information is reasoned by the Pellet reasoner. Nonetheless, the ORO knowledge management system highlights the interaction between robots and humans. SWARMs (Smart and Networking Underwater Robots in Cooperation Meshes) Ontology [25] is built to represent and understand the knowledge of unmanned underwater robots to facilitate cooperation between them. The SWARMs ontology in Reference [13] is divided into four domain-specific ontologies (environmental model, vehicle model, communication model, and mission model) and a core ontology which connects the four domain-specific ontologies. The Semantic Web Rule Language (SWRL) is adopted in the SWARMs ontology for the compensation of the inability of OWL to represent complex rule formations and relations. Reference [26] offers an evaluation of the SWARMs Ontology and verification of applicability by conducting multiple underwater robots cooperation experiments. Comparatively, the ontology in SWARMs is merely constructed for the unmanned underwater robots, so it is hard to extend to other robot application types.

The ontology theory is also applied to other fields, such as industrial production collaboration [27] and navigation in indoor scenarios [28,29]. The work in References [27,30] portrays the domain knowledge for the robot task planning in logic language BC [31,32]. However, this approach is disadvantageous in sharing knowledge and updating the new knowledge inferred. In addition, the ontology representation language OWL [33,34] is endowed with sharing as its natural advantage, which can be easily shared on the web.

Currently, the existing robot ontology knowledge bases embrace the complete representation, which covers each expectation in robot task planning. However, the actual application of the ontology is lacking in intelligence. Meanwhile, the knowledge is just stored in libraries and queried to get the output in use. That is, different knowledge and reasoning lacks associations to obtain new knowledge, like humans' thinking mode.

The main contributions of this paper can be summarized as follows:

The rest of the paper is arranged as follows. Section 2 introduces the purpose and requirements for the building of RTPO. Section 3 presents the building method of RTPO. Section 4 describes the knowledge representation in RTPO. In Section 5, we implement a method to test the scalability and responsiveness of RTPO and an algorithm of robot task planning based on RTPO on a real robot. Finally, Section 6 summarizes all the work and introduces future work.

#### **2. Building Considerations for Robot Task Planning Ontology (RTPO)**

The building considerations for RTPO will be presented in this section. The first part will illustrate the purpose of RTPO building, and then the requirements for RTPO building will be unfolded.

#### *2.1. Purpose of Robot Task Planning Ontology (RTPO)*

It is essential to clarify the purpose of RTPO building. Based on a clear understanding of the purpose, it is possible to decide the contents to be included in the RTPO, which expectations they should be divided into, and which tools should be adopted. The RTPO is mainly associated with knowledge in connection to the robot task planning, including the robot-itself-related concepts, the concepts of environment, and the task-related concepts. Our RTPO design and building aim to provide a comprehensive and available knowledge base for the application of robot task planning. Various heterogeneous robots can query and reason the knowledge from the knowledge base to obtain

useful information, which helps to improve the efficiency of task planning and increase the intelligence by reasoning knowledge to plan automatically.

#### *2.2. Requirements of Robot Task Planning Ontology (RTPO)*

There are many approaches and forms for building the ontology, but there is no unified standard pattern. However, the great ontology should harbor the standard features which should not change with the different approaches and forms. Therefore, the RTPO should meet a set of requirements to ensure an appropriate outcome. Generally, the paper takes into consideration that the great ontology in robotics should cover the following characteristics [35]:

#### **3. The Building of Robot Task Planning Ontology (RTPO)**

This section will give a detailed description of the building method of RTPO, composed of the model, approaches, and knowledge reasoning.

#### *3.1. The Model of Robot Task Planning Ontology (RTPO)*

This section starts with a formal definition of the model in RTPO employed for building the RTPO later on.

**Definition 1.** *A high-level task model is defined as a 5-tuple.*

$$\mathcal{T}' = \left( T\_{\text{name}}, T\_{\text{attr}}, T\_{\text{entity}}, T\_{\text{taskcr}}, T\_{\text{methods}} \right) \tag{1}$$

Consisting of a set of task names *Tname* , a set of task attributes *Tattr*, e.g., the start time and initial state, a set of entities carrying out the tasks *Tentity*, a set of taskers giving the high-level tasks *Ttasker*, and a set of methods to decompose the high-level task *Tmethods*.

**Definition 2.** *an atomic action model is defined as a 5-tuple.*

$$\mathcal{A} = \left( A\_{\text{name}} A\_{\text{attr}} A\_{\text{entity}} A\_{\text{prev}} A\_{\text{effect}} \right) \tag{2}$$

Consisting of a set of atomic actions names *Aname*, a set of atomic actions attributes *Aattr*, a set of entities carrying out the atomic actions *Aentity*, a set of execution preconditions of atomic actions *Apre*, and a set of actions effects *Ae f f ect*.

#### *3.2. The Approaches to Build Robot Task Planning Ontology (RTPO)*

Some software editors are available to edit the ontologies, such as Protégé [36], Neon-tool kit, OntoWiki, and so on [37]. The editor employed for RTPO building is Protégé, equipped with the RDF (Resource Description Framework) triple, which was developed by Stanford University. Besides this, Java is applied as the development language of Protégé. With many embedded plugins in Protégé, the set has been evolved to be one of the essential ontology editors [36]. The Protégé harbors many conception constraints, which helps to add and update the corresponding inferred knowledge. In addition, the knowledge of robot task planning can be derived from Internet, books, or manual editing by humans, just as is shown in Figure 2.

In Figure 2, the query of relevant knowledge from RTPO is realized in the language SWI–Prolog [38]. This logic language, which can be well combined with ROS, can easily and quickly query the relevant knowledge from RTPO. Also, the rules and knowledge can be edited and stored through SWI–Prolog

language. The matching query of knowledge in task planning algorithm later can be conducted through rules.

In the duration of specific implementation, the knowledge is stored in .owl and .pl file formats. Specifically, the .owl file stores the ontology knowledge in RDF triples and the .pl file stores the rules knowledge in Prolog language. RTPO harbors the application in which the paper adopts the ROS as the robot middleware, which is featured by the ROS node and communication through the mechanism of the topic.

OWL, whose file can be generated by Protégé, is further applied as logic language in our research work. An OWL file's generation comes following the ontology building, which holds excellent portability, and can be well used in other approaches. It also breaks down the knowledge interaction barrier among different knowledge systems. OWL file can be published in the World Wide Web and may refer to or be referred from other OWL files [34].

**Figure 2.** The implementation process of RTPO building.

ROS provides the rosprolog function package, through which the users can conveniently explore and debug the knowledge in the terminal window. However, if you would like to apply the knowledge in your robot's control program, you need a way to send queries from your program. This functionality is provided by the json\_prolog package (http://wiki.ros.org/json\_prolog). It provides a service that exposes a Prolog shell via the ROS node. The ROS Node programs can be written in multiple languages, such as Python, C++, and Java. Moreover, the ROS provides a number of function packages for developers. A snapshot of the hierarchy of RTPO is shown in Figure 3.

**Figure 3.** A snapshot of the hierarchy of the Robot Task Planning Ontology (RTPO).

#### **4. Knowledge Representation in Robot Task Planning Ontology (RTPO)**

This section offers the introduction of the knowledge representation in RTPO we have built, including the knowledge types, knowledge structure, and knowledge reasoning in RTPO.

#### *4.1. The Knowledge in Robot Task Planning Ontology (RTPO)*

During the decision process of robots, the task planning module receives the high-level task and subsequently generates a sequence of atomic actions. The execution of atomic actions sequence will have an effect on the environment state, changing the environment state until the goal state is achieved. It is evident that the process of task planning requires knowledge. The knowledge for robot task planning can be composed of two parts. One is the knowledge related to the initial state of the world, which also corresponds to the problem file of task planners. The other is the knowledge relevant to how the given high-level task can be planned from the initial state to the goal state which is also corresponding to the domain file of task planners. The first kind of knowledge exists in the form of world knowledge, including information of environment and robots themselves. The second kind of knowledge is presented in causal knowledge, including the preconditions and the effects on environment state of atomic actions. Taking the high-level task "DeliveryHandbooktoLeo" as an example, the robot task planner calls for the location and capability of robots, the location of humans, the environmental map, and so on, which is defined as world knowledge. Besides this, what the robot needs to know is how to search the optimal sequence of atomic actions from the initial state to the goal state. For example, the task decomposition includes the following steps of "DeliveryHandbooktoLeo": move to Jack, get the handbook, move to Leo, and give the book to Leo. That is the causal knowledge [13].

Similar to a human, when a robot wants to complete a task, first of all, it has to know who it is. That refers to the robot-itself knowledge, including the hardware and software, location information, dynamics information, and so forth. Secondly, in order to complete the task, it also has to harbor the familiarity with the surrounding environment, namely environmental knowledge, which consists of the location and recognition of humans and objects, environment map, information of other robots, and so on. Finally, following the assignment, it needs to understand how to decompose the given high-level task into atomic actions and how to re-plan when the environment is changed, which is the task knowledge.

Therefore, it is easy to understand that nothing else is more important than the three kinds of knowledge representation in our research work. Thus, the paper classifies the RTPO building into three parts: robot ontology, environment ontology, and task ontology, respectively. Next, the paper will describe the structure of RTPO in detail.

#### *4.2. The Structure of Robot Task Planning Ontology (RTPO)*

RTPO contains three parts: robot ontology, environment ontology, and task ontology. The task ontology describes the knowledge connected with robot tasks, such as the task decomposition, task allocation, and task execution, etc. Then, the robot ontology is designed to portray the knowledge or concepts corresponding to the robot itself in the hierarchical structure. Finally, environment ontology gives a description of the knowledge relevant to the environment, such as the environmental map, environmental objects, and so on. The overall structure diagram of the RTPO is shown in Figure 4.

**Figure 4.** The diagram of the whole structure of Robot Task Planning Ontology (RTPO).

#### 4.2.1. Robot Ontology

As shown in Figure 5, the robot ontology contains the following three parts: robots, hardware, and software, which demonstrate the capability and characteristics of robots. Robots include various types of robot, like ground robots, underwater robots, and air robots. The concepts of robots can be installed as individuals in Protégé. The hardware is composed of the components and devices the various types of robots may have. It can be further divided into perception devices, navigation devices, and base devices. Navigation devices refer to the hardware devices that robots need to navigate and locate, such as IMU (Inertial Measurement Unit). Perception devices refer to the hardware devices that robots need to perceive and understand the environment, such as lidar and camera. Base devices refer to the hardware devices related to the low-level control of the robot, such as motor, battery, and so on. Software consists of the functional ROS node, which can publish its specific topic and subscribe to the other topics. It can also achieve communication among control nodes. A variety of relationships can be defined by developers to describe the relationship among the robot ontology and other ontologies.

**Figure 5.** The robot ontology shown in Protégé.

#### 4.2.2. Environment Ontology

Environment ontology is mainly designed for the detailed description of the indoor environment where TurtleBot3 moves. According to the experiment environment, it is seen in Figure 6 that the environment ontology includes the map, obstacles, doors, and other objects in an indoor environment. Besides this, the research requires that the environment ontology should be instantiated partly, for instance, the door and the room. The RTPO is capable of adding and updating environment knowledge after the task planning of robots.

**Figure 6.** The environment ontology shown in Protégé.

#### 4.2.3. Task Ontology

As is displayed in Figure 7, the paper extends the task ontology from four typical tasks, which are, respectively, monitor task, mapping task, delivery task, and charge task. The domain files which most planners use for decomposition are written manually, which is inefficient and less portable. The task description structure is similar to the HTN. Thus, we can decompose the given high-level tasks through ontology for task planning. For better utilizing the hierarchical structures, the representation method of task ontology is designated to meet the experimental requirements. Going forward, the preconditions of each task and sub-action are defined, including the robot's capability and environment state. Besides this, the atomic actions are defined to have effects (delete and add) on the environment state.

**Figure 7.** The task ontology shown in Protégé.

Figure 8 interprets the representation method of atomic actions, defined as the smallest granular actions which can be directly executed by a robot. Atomic actions are made up of execution preconditions and action effects. The former will have an effect on environment state, such as deleting or adding some state, as shown in Figure 8.

**Figure 8.** The representation method of atomic actions.

Generally speaking, for a given high-level task, for example, DeliveryHandbooktoLeo in Figure 9, its representation information in the ontology is insufficient in the initial state, and lacks the update of reasoning knowledge, such as the constraints among different atomic actions. The task knowledge is represented with the insufficient form as follows at the initial state.

**Class**: DeliveryHandbooktoLeo

**SubClassOf**:

DeliveryTask (subAction some GetHandbook) and (subAction some GiveHandbookToLeo) and (subAction some MovetoHandbook) and (subAction some MovetoLeo)

**Figure 9.** The environment state changes in a simple indoor service DeliveryHandbooktoLeo: the TurtleBot3 need to get the handbook from Jack and then give the handbook to Leo.

The robot can adopt a specific task planning algorithm on the basis of the initial state of environment. The task planning algorithm aims to match the preconditions and action effects of atomic actions, so that the execution order constraints can be generated and added into the task ontology. The system can further obtain the atomic action sequence of the specific task. At this moment, new knowledge is obtained and updated based on the reasoning of existing knowledge.

After the task planning algorithm runs, a complete representation of a high-level task decomposition can be obtained and shown as follows, composed of the parent classes, the subactions, and the execution order constraints among subactions. When the same specific task requires planning, the users can query the ontology to obtain the atomic actions sequence of the specific task directly, which will improve the efficiency of task planning.

```
Class: DeliveryHandbooktoLeo
    SubClassOf:
      DeliveryTask
(subAction some GetHandbook)
and (subAction some GiveHandbookToLeo)
and (subAction some MovetoHandbook)
and (subAction some MovetoLeo)
and (orderingConstraints value DeliveryActions12)
and (orderingConstraints value DeliveryActions13)
and (orderingConstraints value DeliveryActions14)
```
and (orderingConstraints value DeliveryActions23) and (orderingConstraints value DeliveryActions24) and (orderingConstraints value DeliveryActions34)

Relying on the individual class in Protégé, the order constraints among sub-actions is defined as follows. Assuming that a specific task has n sub-actions, it is easy to know that the total number to define the specific task is *C* 2 *n*, provided that we completely define the execution order of all sub-actions.

**Individuals**: DeliveryActions12

```
Types:
    PartialOrdering-Strict
```
**Annotations**: occursAfterInOrdering GetHandbook

occursBeforeInOrderingMovetoHandbook

4.2.4. Communications Among the Three Parts

Apart from the contents above, corresponding communications also exist among the three parts which connect this knowledge with another. These relationships can be defined according to the developers' own needs. Taking the indoor service monitor task as an example (Figure 10), the monitor task is achieved by the mobile wheeled robot TurtleBot3 and in the environmental map of Room2. The latter is built by the Lidar of TurtleBot1. Accordingly, these three ontology modules can be linked and added to constraints so that they make up the whole ontology jointly.

**Figure 10.** The communications among three parts in Protégé.

```
4.3. Knowledge Reasoning
```
Figure 11 gives an example of the reasoning in the RTPO knowledge base. The left is the clarification of knowledge and the relationship between the knowledge, but the position of the book is not indicated. That is, we fail to achieve the position of the handbook just through the existing ontology knowledge. On the right, after adding the following rule knowledge, we can infer that the exact location of the handbook is room #1.

```
In (Book, Room) :-
    Has (Human, Book),
    In (Human, Room)
```
**Figure 11.** Example of knowledge reasoning. (**a**) Left: the handbook is asserted to be just a book, and Jack has it. (**b**) Right: If some rule knowledge is added, the knowledge system can infer the location of the handbook.

#### **5. Evaluation and Experiments**

The evaluation of robot knowledge base is a complicated work. So far, there is neither an established benchmark nor evaluation methods. In addition, each robot knowledge system has different coverage and application fields, which brings great challenges to the evaluation of robot knowledge base. It is difficult to evaluate robot knowledge base with only one or a few indicators. Therefore, considering the knowledge system and its application field, this section evaluates RTPO with a combinatorial evaluation method. We adopt a quantitative evaluation method for the scalability and responsiveness of RTPO. Then, this section conducts an experimental case study to verify the usability of RTPO and the flexibility of task planning algorithm based on RTPO on TurtleBot3.

#### *5.1. The Evaluation of Robot Task Planning Ontology (RTPO)*

The scalability of robot knowledge system lies in the efficiency of knowledge updating and storage. The addition of new knowledge is the premise of the application expansion of the knowledge base. The good scalability of knowledge base is the basis of its sustainable development. In order to test the scalability of RTPO, we designed a test algorithm in the process of knowledge base instantiation. The process of knowledge base instantiation refers to the update of individual knowledge, such as the addition of a cabinet and a chair in the indoor environment. Therefore, we designed an algorithm for writing new knowledge to RTPO, which can automatically generate a large number of individuals in a single loop statement:

```
g_individuals (0).
g_individuals (?Num) :-
    new_individuals (?Num).
    ?Num is ?Num−1.
    g_individuals (?Num).
```
The **g\_individuals** (?Num) function just calls the **new\_individuals** (?Num) functions Num times, which then generates Num individuals in RTPO. We used the prolog's **time**() that is ?-**time**(**g\_individuals** (?Num)) to evaluate the performance of **g\_individuals** (?Num). The scalability of RTPO is tested by changing the number of automatically generated individuals and then counting the time consumed by each number of individuals. Figure 12 shows how the consumed time changes

with the number of generated individuals ((blue square markers). It is easy to see that the consumed time increases linearly with the number of generated individuals, where it takes about 2.34 s to generate 55,000 individuals. The maximum generation rate is about 23,500 individuals per second. As a comparison, KnowRob [18] has a maximum generation rate of 22,000 individuals per second and ORO [24] has a maximum generation rate of 7245 individuals per second.

**Figure 12.** The consumed time changes with the number of generated instances (blue square markers) and the response time changes with the number of individual knowledge to query (yellow circle markers).

The responsiveness of knowledge system is mainly reflected in the knowledge query speed. The faster the knowledge query speed, the faster the responsiveness of knowledge system and the better the performance of knowledge system. We choose the Prolog statement "?- **time** (**findall**(?A, **owl\_individual\_of** (?A, 'Obstacles')))" to calculate the query speed of individuals knowledge. As shown in Figure 12 (yellow circle markers), the experimental results show that the response time increases linearly with the increase in the number of individual knowledge to query. Query 52,000 individuals can be completed within 10 s, which ensures the real-time performance of RTPO in the application. Compared with the KnowRob [18], both knowledge systems have similar responsiveness. In a word, RTPO has good scalability and responsiveness.

#### *5.2. Verification Using a Case Study*

In this section, we take the indoor delivery task as a case study to study the robot task planning based on RTPO on TurtleBot3.

#### 5.2.1. Hardware and Software

As is shown in Figure 13, the hardware system in our system is established on TurtleBot3, which is a new generation of mobile robot platform established on ROS (Robot Operation System). The series of TurtleBot from TurtleBot1 to TurtleBot3 is becoming more and more powerful with the ROS. Based on ROS, it is an ideal and essential platform to do research work. Table 1 explains the configuration table of the burger.

**Figure 13.** The TurtleBot3 Burger.

**Table 1.** The configuration table of burger.

Accordingly, the software system is based on ROS, which is the most popular and vital middleware for robot system development. Figure 14 contributes our software system framework, which illustrates that the design and development of ROS nodes contributes to the central part of the software system framework. Based on ROS, the software system framework can be respectively classified into three control levels: control of actions, control of navigation, and control of velocity, corresponding to the task planning module, navigation module, and base control module.

**Figure 14.** The framework of the software system.

#### 5.2.2. The Experimental Scenario

An assumption is made in the case study that the robot is able to grasp objects. In addition, it is noteworthy that the adopted TurtleBot3 in the experiment only harbors mobility and obstacles perception. However, our research attaches importance to the decision-making at the related top level to the robot task planning and does not focus on such basic-level issues as how to control the movement and navigation. Therefore, we make a reasonable assumption that the robot is able to grasp objects.

Under the circumstances of the real robot experiment, the robot needs to get the handbook and then give it to Leo. The built experimental scenario is shown in Figure 15a. The object elements in the environment are displayed in virtue of the label objects on the ground, for example, persons, books and bookshelves. The environment map built through the LIDAR LDS–01 is shown in Figure 15b.

**Figure 15.** The experimental environment we built: (**a**) the real experimental scenario; (**b**) the corresponding environment map built by TurtleBot3 in Gmapping algorithm.

During the verification experiment, the communication among the devices is made through LAN, which shares a master computer and topic to realize the communication requirements between devices. As shown in Figure 16, ontology knowledge is stored in computer PC\_2. Besides this, computer PC\_1 undertakes the master computer. It is also the central computer employed to run the master node. Turtlebot tb\_1 commits the program storage of map building, navigation, and path planning. It is worth noting that the map building algorithm adopts Gmapping; the local path planning algorithm adopts the DWA (Dynamic Window Approach), the global path planning algorithm adopts the D\*, and the location algorithm adopts the amcl.

**Figure 16.** The communication among different devices.

#### 5.2.3. The Experiments and Results

The proposed task planning algorithm Algorithm 1 based on RTPO is shown as the pseudo-code below. Its inputs are the initial state *s*, the given high-level task *t*, and the ontology knowledge *O.* The output of the algorithm is the sequence of atomic actions, which is also the plan for accomplishing the *t* from the initial state.

The specific implementation process of the study case on TurtleBot3 is demonstrated in Figure 17. The atomic actions sequence can be obtained by the task planning algorithm based on RTPO, as shown in Figure 18. The given high-level task "DeliveryHandbooktoLeo", whose initial environment state is "Leo in Room#2; Jack in Room#1; Jack Has Handbook; tb1 in Room#3; tb1 has\_ability DeliveryTask", is decomposed into a sequence of atomic actions: MovetoHandbook, GetHandbook, MovetoLeo, and GiveHandbookToLeo. The corresponding action attributes, such as the target point and the time constraint of some actions, can be obtained by virtue of the analysis of the atomic action list and querying the RTPO. The robot then subscribes to the message and performs the corresponding actions through the navigation and path planning algorithm. Finally, the given high-level task is completed with achieving the goal environment state: "tb1 in Room#2; Leo in Room#2; Leo has Handbook".

**Figure 17.** The specific implementation process of study case on TurtleBot3.

**Figure 18.** The snapshot of the query sentence from the ROS terminal.

Figure 19 shows a sequence of snapshots for the real execution of the atomic action sequence generated by the decomposition of the given high-level task "DeliveryHandbooktoLeo" in a real-world experimental scenario. A sequence of atomic actions obtained from RTPO is displayed as navigating across the target points in a proper order according to atomic action's type. For example, the Moveto action needs to move to the specific target point. Figure 19a shows that the Turtlebot tb\_1 is at the initial position. Figure 19b demonstrates that the Turtlebot tb\_1 has subscribed the topic of atomic action MovetoHandbook and then the atomic action MovetoHandbook is executed. Figure 19c shows that the Turtlebot tb\_1 executes the atomic action MovetoLeo. Figure 19d presents that the Turtlebot tb\_1 has arrived at the position of Leo. The experimental video can be watched at the supplementary material Video S1. The experimental result demonstrates that the robot knowledge base RTPO is of good usability and can be well used in the robot task planning with the proposed task planning algorithm.

**Figure 19.** The sequence of snapshots for the execution of the task: "PutHandbookonBookshelf". (**a**) The initial position of TurtleBot3; (**b**) The atomic action of MovetoHandbook. (**c**) The atomic action MovetoLeo; (**d**) The arrival at the position of Leo.

Besides this, another test scenario is further designed and implemented to further validate the flexibility of task planning algorithm based on RTPO. In this scenario, we suppose that Leo will ask the TurtleBot3 to put the handbook on the bookshelf in room #2 after the robot completed the delivery task "DeliveryHandbooktoLeo". However, in accordance with the topic related to the battery published by the TurtleBot3, the battery of TurtleBot3 becomes insufficient at this time. In this regard, the robot makes a flexible plan for this scenario. First, it needs to recharge its battery in Room #3 and then put the handbook on the bookshelf Room #2, as shown in Figure 20. The given high-level task "PutHandbookonBookshelf" from Leo is decomposed into a sequence of atomic actions: GetHandbook, BatteryCharge, MovetoBookshelf, PutonHandbook. Figure 20e shows that the Turtlebot tb\_1 moves to charge point to have a charge and Figure 20f demonstrates that the Turtlebot tb\_1 moves to the target position of Bookshelf carrying the Handbook. The experimental video can be watched at supplementary material Video S2. The experimental results show that the task planning algorithm based on RTPO has good flexibility to address the unexpected events, such as the insufficient battery.

**Figure 20.** The sequence of snapshots for the execution of the task: "PutHandbookonBookshelf". (**a**) The initial position of TurtleBot3; (**b**) The atomic action of MovetoHandbook. (**c**) The atomic action MovetoLeo; (**d**) The arrival at the position of Leo; (**e**) The atomic action of BatteryCharge; (**f**) The action of MovetoBookshelf.

#### **6. Conclusions**

In conclusion, the paper manages to build a robot ontology called RTPO which is applied to the robots' task planning. Then, RTPO is designed and implemented followed by a proposed evaluation method to test its scalability and responsiveness. The test results show that RTPO has good performance of scalability and responsiveness compared with the existing knowledge base. Finally, we proposed a task planning algorithm based on RTPO and conducted the real robot experiments to verify the usability of RTPO and the flexibility of the proposed algorithm. The experimental results show that the robot can complete the given high-level task smoothly, and can also address unexpected events with good flexibility.

Future research work mainly focuses on the following aspects. Firstly, a large number of available robot ontologies have been constructed, such as KnowRob, ORO, SWARMs, and so on. In this aspect, it is suggested that more consideration of future research should be taken into the fusion of different ontologies. Then, multiple heterogeneous robots can cooperatively help to accomplish some more complex tasks which cannot be accomplished by just a single robot. Thereby, it is of great significance to study cooperative task planning and application based on multi-robot task planning. Additionally, the real world is complex and changeable, due to inaccuracy, randomness, and incompleteness. Therefore, it is necessary to study robot task planning under uncertain environments. Finally, the application of cloud-based knowledge will reduce robots' dependence of specific hardware. It is conducive to the research of multi-agent and swarm intelligence.

**Supplementary Materials:** The following are available online at https://susy.mdpi.com/user/manuscripts/ displayFile/8ec60ffb629e5ad518f79653d69d980d/supplementary, Video S1: DeliveryHandbooktoLeo, Video S2: PutHandbookonBookshelf.

**Author Contributions:** Conceptualization, X.S. and Y.Z.; Formal analysis, X.S.; Funding acquisition, J.C.; Investigation, X.S.; Methodology, X.S. and Y.Z.; Project administration, Y.Z. and J.C.; Resources, J.C.; Supervision, J.C.; Validation, X.S.; Writing – original draft, X.S.; Writing – review & editing, Y.Z. and J.C.

**Funding:** This research was funded by the National Natural Science Foundation of China (Grant No. 61806212, No.61603403, No. U1734208 and No. 61702528).

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **References**

© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## *Article* **Automatic Spray Trajectory Optimization on Bézier Surface**

#### **Wei Chen 1,2, Junjie Liu 1, Yang Tang 3,\* and Huilin Ge 1**

Received: 26 November 2018; Accepted: 18 January 2019; Published: 1 February 2019

**Abstract:** The trajectory optimization of automatic spraying robot is still a challenging problem, which is very important in the whole spraying work. Spray trajectory optimization consists of two parts: spray space path and end-effector moving speed. A large number of spraying experiments have proved that it is very important to find the best initial trajectory of spraying. This paper presents an automatic spray trajectory optimization that is based on the Bézier surface. Spray the workpiece for Bezier triangular surface modeling and find the best initial trajectory of the spraying robot, establish the appropriate spraying model, plan the appropriate space path, and finally plan the trajectory optimization along the specified painting path. The validity and practicability of the method presented in this paper are proved by an example. This method can also be extended to other applications.

**Keywords:** initial trajectory; trajectory optimization; Bézier surface

#### **1. Introduction**

With the development of intelligence, multi-intelligence systems have received extensive attention and application. It can be seen that intelligent automation is widely used in painting robots [1,2]. Surface modeling of the sprayed workpiece is the first step in the trajectory optimization of the spray painting robot and the key to designing the spray path of the spray painting robot. At present, there are two main types of surface modeling methods of sprayed workpiece for off-line programming system of spray painting robot: (1) Surface modeling method based on CAD (Computer Aided Design) model. The modeling method based on the CAD model is that the CAD model data of the workpiece have been obtained before the surface modeling, and the spraying path of the spray painting robot can be planned according to the CAD model of the workpiece. (2) Surface modeling method based on the workpiece scanning system. If there is no CAD model data for a workpiece, or if the surface shape of actual workpiece does not match the CAD model data, then the workpiece needs to be scanned to obtain its new CAD data. The surface of the workpiece is approximated by a simple plane, sphere, cylinder, or other parametric surface, allowing spray path planning on these parametric surfaces.

In the previous work, we focused on the trajectory optimization of complex curved surface. In the reference [3], the Surface modeling method based on the CAD model is adopted. The CAD model data of the workpiece is obtained, and can plan the spraying path of the spraying robot according to the CAD model of the workpiece. The trajectory optimization of spray painting robot for complex curved surface based on the exponential mean Bézier method is proposed. The advantage is that it does not need to split the complex curved surface.

During the off-line programming operation for spray painting robot, after the surface modeling for the workpiece is finished, the following work is to optimize the trajectory of spray painting robot [4,5]. Since the point on the trajectory is a six-dimensional vector in the Cartesian coordinate system, it is very complicated to express it in mathematical expressions and the solving process is very difficult. Therefore, the general idea of trajectory optimization for spray painting robot is usually as follows: First, find the spatial path of the spray painting robot on the workpiece surface, and then find out the optimal time sequence along the specified spatial path. That is, the consistency of the paint thickness on the workpiece surface is the highest and the spray painting time is the shortest at what speed the end effector sprays along the specified spatial path. According to this idea, the optimized trajectory for spray painting consists of two parts: the spatial path of the spray painting operation and the moving speed of the end effector.

On the other hand, a large number of spray painting experiments have shown that the uniformity of the paint thickness can be significantly improved. That is, the spray painting effect can be improved by optimizing the initial trajectory of the spray painting robot in the initial stage of spray painting [6–8]. In other words, finding the best initial trajectory for spray painting is critical to the further trajectory optimization. Therefore, the trajectory optimization for spray painting robot can be divided into the following four steps: (1) Finding the optimal initial trajectory of spray painting robots. (2) According to the geometric features of sprayed workpiece surface, establish a suitable spray painting model. (3) Plan an appropriate spatial path for spray painting. (4) Plan trajectory optimization along the specified painting path.

The initial trajectory selection, the establishment of spraying model and path planning are all the bases of trajectory optimization. This research is based on the assumption that the workpiece CAD model was not acquired in advance. The innovation is that the Bézier triangular surface modeling method is adopted under the CAD model data without the workpiece. Firstly, the Bézier surface is analyzed and the method for searching the optimal initial trajectory of the spray painting robot is given according to the features of the Bézier triangular surface. Subsequently, the spray painting model of Bézier surface is established, and the mathematic expression of paint thickness at a certain point on the Bézier surface is given. Finally, the optimized trajectory of Bézier surface is obtained by using the ideal point method in the mathematical programming with the uniformity of paint thickness and the shortest spray painting time as optimization objectives along the specified spray painting path. The advantage of this method is that a good initial path of automatic spraying is determined at the beginning of the spraying process, which can significantly provide uniformity of coating thickness, that is, improve the spraying effect.

#### **2. Bézier Triangular Surface Modeling Method of Sprayed Workpiece**

Since the Bernstein polynomial has many superior properties, it is widely used in parametric polynomial curve surfaces of many forms. Based on the features of the sprayed workpiece surface, the Bézier triangular surface is constructed by using the Bernstein polynomial as the basis function.

**Definition 1.** *There is an arbitrary given triangle on the plane whose vertices T*1 *T*2*, T*3 *are in the counterclockwise direction. Point P is any point in the plane where the triangle T*1*T*2*T*3 *is located.*

Subsequently, we define:

$$\mu\_1 = \frac{\begin{bmatrix} PT\_2T\_3 \end{bmatrix}}{\begin{bmatrix} T\_1T\_2T\_3 \end{bmatrix}}, \mu\_2 = \frac{\begin{bmatrix} T\_1PT\_3 \end{bmatrix}}{\begin{bmatrix} T\_1T\_2T\_3 \end{bmatrix}}, \mu\_3 = \frac{\begin{bmatrix} T\_1T\_2P \end{bmatrix}}{\begin{bmatrix} T\_1T\_2T\_3 \end{bmatrix}} \tag{1}$$

In the Equation (1), [*T*1*T*2*T*3] represents the directed area of the triangle *T*1*T*2*T*3; When *T*1, *T*2, *T*3 is counterclockwise, [*T*1*T*2*T*3] represents the area of the triangle *T*1*T*2*T*3, that is [*T*1*T*2*T*3] = *S*; When *T*1, *T*2, *T*3 is clockwise, [*T*1*T*2*T*3] represents the opposite number of the area of the triangle, that is, [*T*1*T*2*T*3] = −*S*. Afterwards, we call (*u*1, *u*2, *u*3) as the area coordinate of Point *P*, recorded as *P* = (*u*1, *u*2, *u*3). Triangle *T*1*T*2*T*3 is also called as coordinate triangle, as shown in Figure 1.

**Figure 1.** The Area Coordinate of Point *P* on the Coordinate Triangle.

**Definition 2.** *Suppose the area coordinate of point P on the coordinate triangle T is* (*u*1, *u*2, *u*3)*, and then we define:*

$$B\_{i,j,k}^{n}(P) = \frac{n!}{i!j!k!}u\_1^i u\_2^j u\_3^k \ , \ \ i+j+k=n \tag{2}$$

As the Bernstein basis function ((*n* + 1)(*n* + 2)/2 in all), They have the following properties:

where, according to the triangular theorem we can have:

$$(a+b+c)^n = \sum\_{i+j+k=n} \frac{n!}{i!j!k!} a^i b^j c^k \quad a, b, c \in R, \quad n \in N \tag{3}$$

For any *P*, *u*1 + *u*2 + *u*3 = 1. Let *a* = *u*1, *b* = *u*2, *c* = *u*3, we can have ∑ *i*+*j*+*k*=*n Bn i*,*j*,*k*(*P*) = 1.

Use the any straight line parallel to one side of the triangle to equate the remaining two sides of the coordinate triangle *T* into *n* segments, then the three parallel lines will divide the triangle into *n*2 small congruent triangles, thus we can make the *n*-time subdivision of the coordinate triangle *T*, recorded as *Sn*(*T*). Subsequently, we call each small triangle is the sub-triangle of *Sn*(*T*). The vertices of the sub-triangle ((*n* + 1)(*n* + 2)/2 in all) are called as the node that subdivides *Sn*(*T*). The coordinates of the sub-nodes are as follows:

$$(\frac{i}{n}, \frac{j}{n}, \frac{k}{n}), i+j+k=n \tag{4}$$

Abbreviated as:

$$P\_{i,j,k} = (\frac{i}{n}, \frac{j}{n'}, \frac{k}{n}) \tag{5}$$

**Definition 3.** *Suppose bi*,*j*,*k*(*i* + *j* + *k* = *n*) *is any real number, we call*

$$B^n(P) = B^n(\mu\_1, \mu\_2, \mu\_3) = \sum\_{i+j+k=n} b\_{i,j,k} B^n\_{i,j,k}(P) \tag{6}$$

As the *n*-time Bézier facet on the coordinate triangle *T*, *bi*,*j*,*k* (*i* + *j* + *k* = *n*) as the Bernstein coefficient of the Bézier triangular surface, *Pi*,*j*,*k* = (*Pi*,*j*,*k*; *bi*,*j*,*k*), *i* + *j* + *k* = *n* as the control vertice of the Bézier triangular surface. We call the patch linear continuous function, which is linear on the sub-triangle of *Sn*(*T*) and is the value *bi*,*j*,*k* at node *Pi*,*j*,*k* as the control grid of the Bézier triangular surface.

In particular, for any function *f* : *T* → *R*, the Bernstein coefficient is taken as:

$$b\_{i,j,k} = f(\frac{i}{n'} \frac{j}{n'} \frac{k}{n}) \tag{7}$$

*Electronics* **2019**, *8*, 168

Then we call

$$B^n(P) = B^n(\mu\_1, \mu\_2, \mu\_3) = \sum\_{i+j+k=n} f(\frac{i}{n}, \frac{j}{n}, \frac{k}{n}) B^n\_{i,j,k}(P) \tag{8}$$

as the n-time Bernstein triangular polynomial of *f* on *T*.

Where, in order to simplify the derivation process, three shift operators *E*1, *E*2, *E*3 are introduced, which are defined as:

$$E\_1 b\_{i,j,k} = b\_{i+1,j,k} \tag{9}$$

$$E\_2 b\_{i,j,k} = b\_{i,j+1,k} \tag{10}$$

$$E\_3 b\_{i,j,k} = b\_{i,j,k+1} \tag{11}$$

Subsequently, *Ei* 1*Ej* 2*Ek* 3*b*0,0,0 = *bi*,*j*,*k*, and we have:

$$B^n(P) = \sum\_{i+j+k=n} \frac{n!}{i!j!k!} \mu\_1^i \mu\_2^j \mu\_3^k (E\_1^i E\_2^j E\_3^k b\_{0,0,0}) \tag{12}$$

With the trinomial expansion, Equation (12) can be expressed as:

$$B^n(P) = (\mu\_1 E\_1 + \mu\_2 E\_2 + \mu\_3 E\_3)^n b\_{0,0,0} \tag{13}$$

Accordingly, we have:

$$B^n(T\_1) = E\_1^n b\_{0,0,0} = b\_{n,0,0} \tag{14}$$

$$B^n(T\_2) = E\_2^n b\_{0,0,0} = b\_{0,n,0} \tag{15}$$

$$B^n(T\_3) = E\_3^n b\_{0,0,0} = b\_{0,0,n} \tag{16}$$

Here, we call point *Pn*,0,0 = (1, 0, 0; *bn*,0,0), *P*0,*n*,0 = (0, 1, 0; *b*0,*n*,0), *P*0,0,*n* = (0, 0, 1; *b*0,0,*n*) the corner points of the triangular surface.

When *u*1 = 0, *u*3 = 1 − *u*2. Substituting into Equation (6), then we have:

$$B\_{i,j,k}^{n}(P) = \frac{n!}{j!(n-j)!} \mu\_2^j (1-\mu\_2)^{n-j} = B\_j^n(\mu\_2) \tag{17}$$

Subsequently,

$$B^n(0, \mu\_2, 1 - \mu\_2) = \sum\_{j=0}^n b\_{0, j, n-j} B^n\_j(\mu\_2), 0 \le \mu\_2 \le 1 \tag{18}$$

The boundary of a triangular surface is the n-time Bézier curve with the boundary of triangular surface control grid as the control polygons.

According to the definition of Bézier triangular surface modeling, when *n* = 2, the quadratic Bézier triangular surface generated by six control vertices is:

$$\begin{aligned} B^2(P) &= \sum\_{i+j+k=2} b\_{i,j,k} \frac{2!}{i!j!k!} u\_1^i u\_2^j u\_3^k \\ &= u\_1^2 b\_{200} + u\_2^2 b\_{020} + u\_3^2 b\_{002} + 2u\_1 u\_2 b\_{100} + 2u\_1 u\_3 b\_{101} + 2u\_2 u\_3 b\_{011} \end{aligned} \tag{19}$$

The above Equation (19) can be further expressed as a quadratic form:

$$B^2(P) = \begin{pmatrix} u\_1 & u\_2 & u\_3 \end{pmatrix} \begin{pmatrix} b\_{200} & b\_{110} & b\_{101} \\ b\_{110} & b\_{020} & b\_{011} \\ b\_{101} & b\_{011} & b\_{002} \end{pmatrix} \begin{pmatrix} u\_1 \\ u\_2 \\ u\_3 \end{pmatrix} \tag{20}$$

The resulting quadratic Bézier triangular surface and its control network projection are shown in Figure 2.

When *n* = 3, the cubic Bézier triangular surface that was generated by ten control vertices is:

$$\begin{aligned} B^3(P) &= \sum\_{i+j+k=3} b\_{i,j;k} \frac{3!}{i!j!k!} \mu\_1^i \mu\_2^j \mu\_3^k\\ &= \mu\_1^3 b\_{300} + \mu\_2^3 b\_{030} + \mu\_3^3 b\_{003} + 3\mu\_1^2 \mu\_2 b\_{210} + 3\mu\_1 \mu\_2^2 b\_{120} + 3\mu\_1^2 \mu\_3 b\_{201} \\ &+ 3\mu\_1 \mu\_3^2 b\_{102} + 3\mu\_2 \mu\_3^2 b\_{012} + 3\mu\_2^2 \mu\_3 b\_{021} + 6\mu\_1 \mu\_2 \mu\_3 b\_{111} \end{aligned} \tag{21}$$

The resulting cubic Bézier triangular surface and its control network projection are shown in Figure 3.

**Figure 2.** Quadratic Bézier Triangular Surface and Its Control Network Projection.

**Figure 3.** Cubic Bézier triangular surface and its control network projection.

#### **3. Optimal Initial Trajectory Selection for Automatic Spraying on Bézier Surface**

#### *3.1.  *Vi*,*j*(*i* = 0, 1, ··· , *m*; *j* = 0, 1, ··· , *n*) is the control vertice or Bézier point of the curved surface. The control vertices form *m* + 1 and *n* + 1 control polygons along the v-direction and u-direction, respectively, which together form a curved control grid or a Bézier grid.

The properties of the Bézier surface are as follows:

(1) The four corner points of the Bézier grid are the four corner points of the Bézier surface:

$$B(0,0) = V\_{0,0}, \\ B(1,0) = V\_{\text{m},0\prime} \\ B(0,1) = V\_{0,\text{n}}, \\ B(1,1) = V\_{\text{m},\text{n}} \tag{23}$$

(2) The outermost vertex of Bézier grid defines the four borders of the Bézier surface and it has the following characteristics at the boundary, as shown in Table 1:

**Table 1.** Characteristics at the Boundary of Bézier Grids.

#### *3.2. Optimal Initial Trajectory Selection*

A large number of spray painting practical applications show that in the beginning of the spray painting operation if we can determine a good initial trajectory of spray painting robot, the uniformity of paint thickness can be significantly improved. That is to say, the spray painting effect can be improved. It can also reduce the spray painting time, improve the spray painting efficiency, and reduce the rate of paint waste at the same time. At present, the optimal initial trajectory selection method of the existing painting robot is to use the plane cutting method to take the obtained cross line as the initial trajectory of the painting robot [9–11]. This method can improve the spray painting effect to a certain extent, but the randomness is large and the spray painting time cannot be optimized. In this paper, the initial trajectory selection method that is based on geodesic curvature can not only improve the spray painting effect (paint thickness uniformity), but also can improve the spray painting efficiency (reduce spray painting time).

The curvature of a certain point on the painting path can be divided into two kinds: The first one is used to characterize the bending degree of the painting path passing through the point along normal vector of the surface, which is called the normal vector curvature. The second is used to characterize the extent to which the spray trajectory bends to the boundary line of the surface, called geodesic curvature. As shown in Figure 4, Figure 4a is a zero-geometric curvature trajectory. That is, the paint thickness is uniform on the both side of the painting path. Figure 4b is the spray painting trajectory whose geodesic curvature is a constant. It is obvious that more paint is accumulated in the direction

where the trajectory is bumped. Figure 4c shows the painting path of the variable geodesic curvature. The paint thickness on both sides of the trajectory is not uniform. It can be seen that the changing rate of the geodesic curvature at each point on the painting path has an influence on the paint thickness. In order to improve the consistency of the paint thickness, the changes in geodesic curvature at each point on the trajectory must be taken into account when selecting the initial spray painting trajectory.

**Figure 4.** The Influence of Geodesic Curvature on the Consistency of Paint Thickness.

From the example shown in Figure 4, it can be seen that the zero-geometric curvature curve should be selected when determining the initial trajectory, and the initial trajectory also determines the geodesic curvature of the subsequent trajectory. The process of selecting the initial trajectory for spray painting can be divided into two steps: 1) Determining the relative position of the initial spray painting trajectory and the boundary of the workpiece surface. 2) Selecting the direction of the initial spray painting trajectory. As shown in Figure 5a, the initial trajectory is a geodesic, but the geometric curvature is very high when the offset curve passes the area near the apex of the cube. In Figure 5b, the initial trajectory is also the geodesic, but the surface is symmetrically bisected into two parts with the same Gaussian curvature being integral. Thus, in determining the position of the initial trajectory, it is necessary to select a position that minimizes the geodesic curvature of the offset curve.

**Figure 5.** The Influence of the Relative Position of Geodesic Curve and Surface Boundary on Subsequent Trajectory.

#### 3.2.1. Determining the Relative Position of Initial Trajectory

As shown in Figure 6, let a segment on the smooth initial trajectory *α*0 as *Cst*. We use *α*0(*t*0) and *α*0(*t*1) to represent the two endpoints of *Cst*. The offset curve of line segment *Cst* is generated by measuring the distance between the offset curve and the initial trajectory according to the geodesic lines γ*t*0 and γ*t*1 , which are perpendicular to the initial trajectory at points *α*0(*t*0) and *α*0(*t*1). Assume that *Co f* is the offset curve with a distance Δ of *Cst* and the offset distance Δ is less than the focal length of *α*0. Here, we only need to consider the integration of the geodesic curvature along *Co f* . Accordingly, we can assume that the surface is continuous. Subsequently, we can assume that *Co f* , γ*t*0 , and γ*t*1 are all smooth curves. Assume that *φ* is a region enclosed by boundaries *Cst*, *Co f* , γ*t*0 , and γ*t*1 , and an arbitrary smooth curve connecting γ*t*0 (Δ) and γ*t*1 (0) is *C*dia. Suppose that the region bounded by *Cst*, γ*t*0 , and *C*dia is *φ*1, its boundary *∂φ*1 contains the curves *Cst*, γ*t*0 , *C*dia, and their corresponding

directions. Similarly, *φ*2 denotes the region bounded by *Co f* , γ*t*1 , and *C*dia, and *∂φ*2 is the boundary of *φ*2.

**Figure 6.** The Relationship between the Curvature Integral of Geodesic Surface and the Gaussian Curvature of Surfaces.

By applying the Gauss-Bonnet formula to the triangular regions *φ*1 and *φ*2, we can obtain that:

$$\bigcup\_{\phi\_i} \mathcal{K}d\sigma + \oint\_{\partial\phi\_i} k\_{\mathcal{S}} ds = \sum\_{j=1}^{3} \theta\_{i,j} - \pi, \; i = 1, 2 \tag{24}$$

In the expression above, *K* is the Gaussian curvature of the surface *φi*. *kg* is the geometric curvature of the triangle boundary *∂φi*. *θi*,*j* is the *j*-th interior angle of the boundary *∂φi*. As γ*t*0 and γ*t*1 are geodesic, so the integrals γ*t*0 *kgds* and γ*t* 1 *kgds* are zero. Subsequently, we have:

$$\begin{split} \int\_{\mathbb{S}\phi\_{1}} k\_{\mathcal{S}} ds + \oint\_{\mathbb{S}\phi\_{2}} k\_{\mathcal{S}} ds &= \\ \left( \int\_{\mathbb{C}\_{st}} k\_{\mathcal{S}} ds - \int\_{\mathbb{Y}\_{t\_{0}}} k\_{\mathcal{S}} ds - \int\_{\mathbb{C}\_{dis}} k\_{\mathcal{S}} ds \right) + \left( \int\_{\mathbb{Y}\_{t\_{1}}} k\_{\mathcal{S}} ds + \int\_{\mathbb{C}\_{dis}} k\_{\mathcal{S}} ds - \int\_{\mathbb{C}\_{sf}} k\_{\mathcal{S}} ds \right) \end{split} \tag{25}$$

$$
\oint\_{\partial\phi\_1} k\_{\mathfrak{J}} ds + \oint\_{\partial\phi\_2} k\_{\mathfrak{J}} ds = \int\_{C\_{st}} k\_{\mathfrak{J}} ds - \int\_{C\_{of}} k\_{\mathfrak{J}} ds \tag{26}
$$

Obviously, *φ*1 *Kdσ* + *φ*2 *Kdσ* = *φ Kdσ*, γ*t*0 , and γ*t*1 are perpendicular to the seed curve, so we have: *θ*1,1 + *θ*2,3 = *θ*1,2 = *π* 2 , Substituting into expression (26), then we sum the triangular regions *φ*1 and *φ*2:

$$\int\_{\mathcal{C}\_{\sigma f}} k\_{\mathfrak{J}} ds = \iint \Phi \, \mathsf{K} d\sigma + \int\_{\mathcal{C}\_{st}} k\_{\mathfrak{J}} ds \tag{27}$$

Finally, if the initial trajectory is a geodesic, then:

$$\int\_{C\_{of}} k\_{\S} ds = \iint \, \oint d\sigma \,\tag{28}$$

#### 3.2.2. Selecting the Direction of Initial Path

In order to ensure that the time along the initial trajectory is the least, when selecting the spatial direction of the initial trajectory, it is necessary to select a curve from numerous geodesic curvature and Gaussian curvature curve on the surface whose integrals are equal to that of an initial trajectory. We can give the definition of surface height first, and then determine the direction of the initial trajectory according to the definition. The surface height is the sum of the longest geodetic curve (straight geodesic) perpendicular to the initial trajectory that extends from the initial trajectory to both sides, as shown in Figure 7. The optimal initial trajectory is the initial trajectory corresponding to the minimum surface height of the surface. With the same spray painting distance, the reciprocating spray painting time along the optimal initial trajectory is the least (Figure 8). What is more, the paint consistency is the best and the paint consumption is also the least.

**Figure 7.** Surface Height Measurement Method.

**Figure 8.** Relationship between Surface Height and Reciprocating Spray Time.

#### **4. Automatic Spray Space Path Generation on Bézier Surfaces**

Under normal circumstances, using the Bézier method to generate the surface is more complex. It is more complex if we perform optimization of spraying trajectory directly on Bézier surface. On the other hand, during the spraying operation, the distance between the end effector and the workpiece surface is always constant and perpendicular to the workpiece surface. In this case, the end effector of the spray equipment is essentially reciprocating on the isometric surface of the workpiece surface. Therefore, according to this idea, we can first find its isometric surface according to the shape of the surface and then perform optimization of spraying trajectory on the isometric surface. It should be noted that, strictly speaking, it is the discrete point array of the isometric surface of the Bézier surface but not the isometric surface of Bézier surface that we will find out according to this method. In the spatial path planning of the spray painting robot, we only need to find the discrete points on the path essentially and then fit out the spray path using the corresponding mathematical methods according to the accuracy requirements.