Therefore, with the given definition of *A* and *b*, the optimal control will minimize the motor commands while performing the tracking in the image space. The final control law can be obtained replacing these variables into the function that minimizes the motor signals described by Equation (22):

$$\boldsymbol{\Upsilon} = \mathbf{W}^{-1/2} \left( \mathbf{J}\_i \boldsymbol{\tilde{M}}^{-1} \mathbf{W}^{-1/2} \right)^{+} \cdot \left( \ddot{\mathbf{s}}\_r - \dot{\mathbf{J}}\_i \boldsymbol{u} + \mathbf{J}\_i \boldsymbol{\tilde{M}}^{-1} \boldsymbol{\tilde{C}} \right) \tag{25}$$

As it can be seen, the visual controller represented by (25) implicitly depends on the weighting matrix **W,** and different values of this matrix can be used to coordinate the motion of the base and of the manipulator. The diagram of the considered control loop is depicted in Figure 3.

**Figure 3.** Diagram of the control scheme for the mobile manipulator robot.

Considering **W** = *M* −2 and replacing this value in the control framework expressed in Equation (25), the result yields

$$\widetilde{\mathbf{T}} = \, \breve{\mathbf{M}} \mathbf{J}\_i^+ \cdot \left( \ddot{\mathbf{s}}\_{\mathbf{r}} - \dot{\mathbf{J}}\_i \boldsymbol{u} + \mathbf{J}\_i \breve{\mathbf{M}}^{-1} \breve{\mathbf{C}} \right) \tag{26}$$

This controller represents a direct visual servo control using inversion of the dynamic model. Additionally, new controllers can be obtained using different values of **W**. For example, an important value for **W**, due to its physical interpretation, is **W** = *M* −1 , since it is consistent with the principle of d'Alembert:

$$\tilde{\pi} = \tilde{\boldsymbol{M}}^{1/2} \left( \boldsymbol{J}\_i \tilde{\boldsymbol{M}}^{-1} \tilde{\boldsymbol{M}}^{1/2} \right)^+ \cdot \left( \ddot{\mathbf{s}}\_r - \dot{\boldsymbol{J}}\_i \boldsymbol{u} + \boldsymbol{J}\_i \tilde{\boldsymbol{M}}^{-1} \tilde{\mathbf{C}} \right) \tag{27}$$

Now, the definition of the reference control, .. *s*r, is described considering the eye-in-hand camera system that extracts a set of k image feature points. The task description as a constraint is given by the following equation in the image space.

$$\left(\ddot{\mathbf{s}}\_d - \ddot{\mathbf{s}}\right) + \mathbf{K}\_D \left(\dot{\mathbf{s}}\_d - \dot{\mathbf{s}}\right) + \mathbf{K}\_P (\mathbf{s}\_d - \mathbf{s}) = 0\tag{28}$$

where .. *sd*, . *sd*, and *sd* are the desired image space accelerations, velocities, and positions, respectively. *KP* and *KD* are proportional and derivative gain matrices, respectively. This equation can be expressed in regards to image error in the following way.

$$
\ddot{\mathbf{s}}\_d + \mathbf{K}\_D \dot{\mathbf{e}}\_\mathbf{s} + \mathbf{K}\_P \mathbf{e}\_\mathbf{s} = \ddot{\mathbf{s}}\_r \tag{29}
$$

where *es* and . *es* are the image error and the time derivative of the image error, respectively. As stated, the variable .. *sr* denotes the reference image accelerations of the proposed image space based controller. Substituting this variable into the dynamic visual servo controller in Equation (25), the control law is set by the following relationship.

$$\widetilde{\mathbf{r}} = \mathbf{W}^{-1/2} \left( I\_i \widetilde{\mathbf{M}}^{-1} \mathbf{W}^{-1/2} \right)^+ \cdot \left( \ddot{\mathbf{s}}\_d + \mathbf{K}\_D \dot{\mathbf{e}}\_s + \mathbf{K}\_P \mathbf{e}\_s - \dot{I}\_i u + I\_i \widetilde{\mathbf{M}}^{-1} \widetilde{\mathbf{C}} \right) \tag{30}$$

In order to demonstrate the asymptotic tracking of the control law, some operations must be done. First, the closed-loop behavior is computed from Equation (19) as

$$\delta \mathbf{M} \dot{\mathbf{u}} + \tilde{\mathbf{C}} = \mathbf{W}^{-1/2} \left( I\_i \tilde{\mathbf{M}}^{-1} \mathbf{W}^{-1/2} \right)^{+} \cdot \left( \bar{\mathbf{s}}\_d + \mathbf{K}\_D \dot{\mathbf{e}}\_s + \mathbf{K}\_P \mathbf{e}\_s - \dot{\mathbf{J}}\_i \mu + I\_i \tilde{\mathbf{M}}^{-1} \tilde{\mathbf{C}} \right) \tag{31}$$

Equation (31) can be simplified by premultiplying its left and right sides by *JiM* −1 **W**−1/2 **W**1/2:

$$J\_i \dot{u} = \ddot{\mathbf{s}}\_d + \mathbb{K}\_D \dot{\mathbf{e}}\_\mathbf{s} + \mathbb{K}\_P \mathbf{e}\_\mathbf{s} - \dot{f}\_i u \tag{32}$$

Finally, using the relationship expressed in (23), it can be concluded that

$$
\ddot{\mathbf{e}}\_{\mathfrak{s}} = -\mathbf{K}\_{D}\dot{\mathbf{e}}\_{\mathfrak{s}} - \mathbf{K}\_{P}\mathbf{e}\_{\mathfrak{s}} \tag{33}
$$

Therefore, when *Ji* is full-rank, an asymptotic tracking is achieved by the visual servo controller expressed in Equation (30) for the tracking of an image trajectory.

#### **6. Results**

This section presents the system behavior during the tracking of different trajectories. The robot is guided by an eye-in-hand camera system. The parameters of a Gigabit Ethernet TM6740GEV camera is considered, which acquires 200 images every second with a resolution of 1280 × 1024 pixels. The eye-in-hand camera extracts four visual features from the four corners of the target.

Three different experiments are presented in this section. In these experiments the value of **W** = *M*−1 is considered. Figure 4a,b represents the initial configuration employed in all of the experiments. Figure 4a represents the target and the mobile manipulator with the eye-in-hand camera system. The initial position of the visual features is represented in Figure 4b. The desired trajectory employed in the first experiment is a linear trajectory that represents a lateral translation of 200 px. in x and y directions in the image from the initial position of the visual features. As it can be seen in Figure 4d, the image-based controller correctly carries out the tracking and a straight line is obtained in the image space. Figure 4c represents the final pose of the mobile manipulator. The second experiment, represented in Figure 4e,f, evaluates the tracking when a displacement in depth must be performed. As it can be seen in Figure 4e, a displacement in depth is carried out and, once the experiment ends, the mobile manipulator is closer to the target. Figure 4f represents the obtained image trajectory. This trajectory corresponds to the desired trajectory specified as a linear increase in depth from the initial image configuration. Finally, the experiment represented in Figure 4g,h requires a change in orientation of the eye-in-hand camera with respect to the observed target. To do this, the desired image trajectories for the visual features are represented by a straight line between the initial and the final ones defined as *s*∗ = [*f*1*d* = (503, 584), *f*2*d* = (612, 600), *f*3*d* = (523, 472), *f*4*d* = (630, 477)]*T* px. As in the previous experiments, Figure 4g represents the final robot pose once the experiment ends and Figure 4h represents the image trajectory. As the experiments show, the proposed controller allows the concurrent guidance of both the manipulator and robotic platform during the tracking of the desired image trajectory.

**Figure 4.** Robot pose and image trajectories in the experiments. (**a**) 3D initial pose of the mobile manipulator and target. (**b**) Initial visual features extracted by the eye-in-hand camera. First experiment: (**c**) 3D final pose. (**d**) Image trajectory. Second experiment: (**e**) 3D final pose. (**f**) Image trajectory. Third experiment: (**g**) 3D Final pose. (**h**) Image trajectory.

Figure 5 represents the joint torques applied to the manipulator (m = 1 ... 7) and force and moments applied to the robot platform during the first and second experiments. As it can be seen in Figure 5a (first experiment), the torques, force, and moment remain low, and a smooth behavior is obtained during the tracking. For the second experiment, Figure 5b represent the joint torques applied to the manipulator and force and moment applied to the base platform during the robot motion in depth. It is worth noting that the reduction of the distance between the two bodies (target and robot) is obtained by modifying both the base platform pose and the robot manipulator joint configuration. Finally, as in the previous experiments, Figure 6 represents the joint torques applied to the manipulator and force and moment applied to the base platform during the third tracking experiment. The desired relative orientation is achieved by controlling both the orientation of the base platform and the robot manipulator joint configuration.

**Figure 5.** (**a**) Experiment 1: Joint torque and moment and force exerted on the base platform. (**b**) Experiment 2: Joint torque, moment, and force exerted on the base platform.

**Figure 6.** Experiment 3: Joint torque, moment, and force exerted on the base platform.

One of the main advantages of the proposed controller is the possibility to define new direct image-based controllers by modifying the value of the matrix **W** of the optimal controller (see Equation (25)). A different controller with different dynamic behavior during the tracking of image trajectories can be obtained by modifying this matrix. In order to evaluate the proposed framework, in Table 1 we represent the obtained image error during the tracking of a repetitive image trajectory defined by the following equation.

$$\mathbf{s}\_d = \begin{bmatrix} f\_{xd} \\ f\_{yd} \end{bmatrix} = \begin{bmatrix} 320 + 166 \cos(\omega t + \pi/4) \\ 265 + 160 \sin(\omega t + \pi/4) \end{bmatrix} \tag{34}$$

**Table 1.** Mean image error in pixels (mm) during the tracking of the image trajectory.

Table 1 represents the mean error in pixels and mm during the tracking of the image trajectory considering different tracking velocities. As it can be seen in this table, different tracking errors are obtained and the best behavior at high velocities is obtained considering **W** = *M* −1 . Additionally, the performance of the controllers is compared with respect to previous controllers only based on inverse dynamics [22] (indicated as "previous" in Table 1).

#### **7. Conclusions**

This paper presents a direct image-based visual servoing system for the guidance of mobile manipulators. This approach is based on an optimal framework that considers the optimization of the motor signals sent to the mobile manipulator during the tracking of image trajectories. The stability of the controller has been proved analytically, and different dynamical behavior can be obtained by using different values of the matrix **W**.

As a result, an adaptive and flexible tool has been obtained, allowing for concurrent control of both the base platform and manipulator during the tracking of trajectories. With the given definition of *A* and *b*, the optimal control can minimize the motor commands while performing the tracking of trajectories in the image space. The viability of the controller has been tested in a variety of test case experiments, including approaches to an object, as well as different positioning tasks that require modifying the position and/or the orientation of the mobile manipulator. In all such cases, the experiments and trajectories have been accomplished.

**Author Contributions:** Funding acquisition, C.A.J.; Methodology, J.L.R.; Software, Á.B.; Supervision, J.P.; Validation, G.J.G.

**Funding:** This research was supported by the Valencia Regional Government through project GV/2018/050. **Conflicts of Interest:** The authors declare no conflict of interest.

#### **Appendix A**

As described throughout the paper, the motor commands are *u*= *uT p uT m T* = *v ω* . *q T m T* . First, the robot Jacobian is used to convert the joint velocities to Cartesian velocities:

$$
\begin{bmatrix}
v \\ \omega \\ \dot{x}\_{mc} \\ \dot{y}\_{mc} \\ \dot{z}\_{mc} \\ \dot{\Phi}\_{xmc} \\ \dot{\Phi}\_{ymc} \\ \dot{\Phi}\_{zmc}
\end{bmatrix} = \begin{bmatrix}
I\_{2\times2} & \mathbf{0}\_{2\times6} \\
\mathbf{0}\_{6\times2} & I\_{6\times6}
\end{bmatrix} \mu. \tag{A1}
$$

The relation between the previous Cartesian velocities and *vT ωT T* can be obtained by using Equations (11)–(16):

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

$$
\begin{bmatrix}
\dot{t}\_x\\ \dot{t}\_y\\ \dot{t}\_z\\ \dot{\Phi}\_x\\ \dot{\Phi}\_y\\ \dot{\Phi}\_z
\end{bmatrix} = \begin{bmatrix}
v\\ \omega\\ \dot{x}\_{mc}\\ \dot{y}\_{mc}\\ \dot{z}\_{mc}\\ \dot{\Phi}\_{xmc}\\ \dot{\Phi}\_{ymc}\\ \dot{\Phi}\_{zmc}
\end{bmatrix} \tag{A2}
$$

Where,

$$\begin{aligned} \mathbf{J}\_{c1} &= \begin{bmatrix} \cos a & -\mathbf{x}\_{m}\sin a - \mathbf{x}\_{pun}\sin a & \cos a & -\sin a & 0 & 0 & 0 & 0\\ \sin a & \mathbf{x}\_{m}\cos a + \mathbf{x}\_{pun}\cos a & \sin a & \cos a & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix} \\ \mathbf{J}\_{c2} &= \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & \cos a & -\sin a & 0\\ 0 & 0 & 0 & 0 & 0 & \sin a & -\cos a & 0\\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned} \tag{A3}$$

Finally, by using the rotation matrix *Rc* which represents the orientation frame F*c* with respect to the fame F*o*, the values of *vT ωT T* can be expressed with respect to the frame F*c*, *vT c ωT c T* .

$$
\begin{bmatrix} \boldsymbol{\sigma}\_{\boldsymbol{\varepsilon}} \\ \boldsymbol{\omega}\_{\boldsymbol{\varepsilon}} \end{bmatrix} = \begin{bmatrix} \boldsymbol{R}\_{\boldsymbol{\varepsilon}}^{T} & \mathbf{0} \\ \mathbf{0} & \boldsymbol{R}\_{\boldsymbol{\varepsilon}}^{T} \end{bmatrix} \begin{bmatrix} \boldsymbol{\sigma} \\ \boldsymbol{\omega} \end{bmatrix} \tag{A4}
$$

Therefore, the value of *Jc* is equal to

$$J\_c = \begin{bmatrix} R\_c^T & \mathbf{0} \\ \mathbf{0} & R\_c^T \end{bmatrix} \begin{bmatrix} J\_{c13\times 8} \\ J\_{c23\times 8} \end{bmatrix} \begin{bmatrix} I\_{2\times 2} & \mathbf{0}\_{2\times 2} \\ \mathbf{0}\_{6\times 2} & J\_{6\times 6} \end{bmatrix} \tag{A5}$$

#### **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* **Picking Robot Visual Servo Control Based on Modified Fuzzy Neural Network Sliding Mode Algorithms**

#### **Wei Chen 1,2,\*, Tongqing Xu 1, Junjie Liu 1, Mo Wang 2 and Dean Zhao 3**

Received: 23 April 2019; Accepted: 24 May 2019; Published: 29 May 2019

**Abstract:** Through an analysis of the kinematics and dynamics relations between the target positioning of manipulator joint angles of an apple-picking robot, the sliding-mode control (SMC) method is introduced into robot servo control according to the characteristics of servo control. However, the biggest problem of the sliding-mode variable structure control is chattering, and the speed, inertia, acceleration, switching surface, and other factors are also considered when approaching the sliding die surface. Meanwhile, neural network has the characteristics of approaching non-linear function and not depending on the mechanism model of the system. Therefore, the fuzzy neural network control algorithm can effectively solve the chattering problem caused by the variable structure of the sliding mode and improve the dynamic and static performances of the control system. The comparison experiment is carried out through the application of the PID algorithm, the sliding mode control algorithm, and the improved fuzzy neural network sliding mode control algorithm on the picking robot system in the laboratory environment. The result verified that the intelligent algorithm can reduce the complexity of parameter adjustments and improve the control accuracy to a certain extent.

**Keywords:** visual servoing; fuzzy neural network; sliding mode control; picking robot

#### **1. Introduction**

With the development of science and technology, robot technology has acquired unprecedented achievements. In agriculture, the development of fruit-picking robots has become a hotpot project due to the rising labor costs and the huge output of apples and citrus. The apple-picking robot is an integrated system that integrates environmental perception, dynamic decision-making, and behavior control; motion control is therein the most basic and important link. At present, there are two main kinds of visual servo control for picking robots: one is image-based visual servo control, and the other is a position-based visual servo control system [1]. The picking robot manipulator is a typical non-linear system, because it is composed of multiple joints and has time-varying uncertainty during the picking process. This indicates that traditional control methods would not meet the control requirements. Image feedback-based visual servo control is a real-time feedback control for the manipulator according to the visual acquisition image, and it also shows strong self-adaptability to the change of external environment and system parameters so that it reduces the complexity of manipulator in the control process.

At present, most visual servo control systems based on image feedback adopt traditional control schemes such as adaptive control [2], PID control [3], and sliding model control [4], etc., or intelligent control algorithms [5] such as neural network and fuzzy theory [6], etc. There are some defects in adopting a single control method. Adaptive control is a strategy to deal with structural uncertainties, which provides good performance to a certain extent [7]. PID control has better control performance for the position and joint angle parameters of the manipulator end-effector [8]. Sliding-mode control (SMC) has excellent robustness and good anti-jamming ability to the uncertainties of the system, especially in the control of non-linear systems [9]. Neural network (NN) has the ability to learn and approximate non-linear functions [10]. It is often used to compensate unstructured uncertainties in robot controller modeling. Fuzzy control [11] imitates human reasoning and decision-making processes on the basis of not depending on the precise mathematical model of the controlled object. Fuzzy logic is adopted in robot controllers to achieve good control performance under uncertain conditions. In the reference [12], a robust adaptive control method based on the dynamic structure of the fuzzy wavelet neural network is proposed, which is used to track the trajectory of the robot with uncertainties and disturbances by the adaptive sliding-mode control. Mehta and Ton et al. introduced a continuous terminal sliding mode visual servo controller to adjust the camera to the desired position, but this has many requirements for the camera, and requires the correction of the camera position [13]. In the field of robotic citrus picking, there is a non-linear robust image visual servo controller that can adjust the end effector to the fruit position under the condition of unknown fruit movement, but the speed of the picking process is slow, and is not conducive to agricultural application [14]. The fuzzy neural network (FNN) learning algorithm [15,16] is a superior system identification method that utilizes non-linear control. There is no need to get the prior knowledge about the uncertainty and a sufficient amount of observed data. Efe and Devesh et al. used the novel particle swarm optimization (PSO)–SMC hybrid learning algorithm to train the FNN effectively, but instead of using the visual servo control of picking robots, they realized the flattening and rotation control of the four-rotor configuration for agricultural purposes [17].

A great deal of research has been devoted to these intelligent algorithm scholars, but there are few applications in the field of agricultural picking robots. In this paper, a joint visual servo control algorithm based on an improved sliding mode and kinematics and dynamics equation is presented that appears to have good robustness in a non-linear system. However, sliding-mode control always requires detailed system information and corresponding uncertainty bounds to ensure stability [18]. Even though the stability requirements are met, there will still be problems such as jitter [19]. Therefore, fuzzy and adaptive neural network control algorithms are introduced and combined with sliding-mode control to suppress the sliding-mode jitter problem. However, the traditional neural network model is complex and computationally intensive, with a high probability of over-fitting. To prevent over-fitting, we increased the dropout rate for the dropout layer [20].

According to the characteristics of apple-picking robot motion control, the image-based robot visual servo control method is combined with the improved fuzzy neural network sliding mode control algorithm to make use of the non-linear and non-system-dependent mechanism model of the fuzzy neural network. Combined with sliding mode variable structure control, the sliding mode chattering is eliminated, and the stability of the control system is improved. In this paper, the algorithm is applied for the first time in the field of agricultural fruit-picking robots, which improves the extraction speed of apple-picking robots.

The research aim of this paper is mainly to verify the visual servo control of the sliding-mode control algorithm based on the improved fuzzy neural network through the apple-picking robot. The article is divided into three parts, Firstly, we establish the kinematics and dynamics equations of the picking robot. Secondly, visual positioning is introduced to calculate the position of the target point, and an image-based visual control algorithm is used. Finally, the sliding-mode control algorithm and the improved fuzzy neural network control algorithm are combined to carry out simulation analysis, and the algorithm is verified on the picking robot. It is concluded that the improved fuzzy neural network sliding mode control algorithm can improve the efficiency of the robot arm servo control and has higher stability.

#### **2. Picking Robot Structure and Model Establishment**

#### *2.1. Manipulator Structure Design*

Since the apple trees are relatively high and the fruits are disperse, the higher degree of freedom of the picking robot manipulator brings not only the kinematics algorithm complexity, but also the waste of joint freedom degree, which is disadvantageous to the practicality of agriculture. This reduces the requirements of the manipulator and minimizes the usage of freedom degrees. So, this paper introduces the self-designed five degrees of freedom (DOF) manipulator, which is shown in Figure 1. Figure 2 is a schematic view of the picking robot manipulator. The manipulator gripping device adopts a curved design in order to reduce the damage of the apple during the process of picking up the apple. Figure 3 shows a wheeled picking robot.

**Figure 1.**

**Figure 2.** Schematic diagram of picking robot manipulator.

**Figure 3.** Picking robot physical map.

Fruit-picking robots are mainly composed of two parts. The first part is a chassis platform, and the second part is a manipulator. Therein, the five DOF manipulator is a PRRRP joint structure. The first degree of freedom is the lifting platform. Its main function is to lift the whole manipulator. The second degree of freedom is the waist rotation joint of the manipulator. The third degree of freedom is the swing axis of the arm, the fourth degree of freedom is the swing axis of the arm, and the fifth degree of freedom is the rotating axis of the manipulator with the function of adjusting the grasping posture according to the picking requirement.

#### *2.2. Kinematics Equation of Manipulator*

The manipulator is a complex system, since it includes various disciplines such as kinematics and dynamics. Kinematics analysis is used to model according to the relationship between the spatial posture and the angle of each joint of the manipulator. The designed structural kinematics parameters of the manipulator are shown in Table 1. In the table, α*i* is the torsion angle, *ai* is the length of the connecting rod, θ*i* is the joint angle, *di* is the offset of the connecting rod, σ = 0 is the rotating joint, σ = 1 is the direct acting joint, and *di* is the other constant. The use of the D–H rule to determine the coordinate position of the connecting rod is shown in Figure 4.

**Table 1.** Kinematic parameters of manipulators.

**Figure 4.** Manipulator D–H model.

The homogeneous coordinate transformation matrix is used in the above-mentioned establishment of connecting the rod D–H coordinate system and kinematics mode.

$$\begin{aligned} \;\_{1}^{0}T &= \begin{bmatrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 1 \\ 0 & 0 & 1 & d\_{1} \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \;\_{2}^{1}T &= \begin{bmatrix} \cos\theta\_{2} & 0 & -\sin\theta\_{2} & 0.133\cos\theta\_{2} \\ \sin\theta\_{2} & 0 & \cos\theta\_{2} & 0.133\sin\theta\_{2} \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \;\_{3}^{2}T &= \begin{bmatrix} \cos\theta\_{3} & -\sin\theta\_{3} & 0 & \cos\theta\_{3} \\ 1 & \cos\theta\_{3} & 0 & \sin\theta\_{3} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \;\_{4}^{3}T &= \begin{bmatrix} \cos\theta\_{4} & 0 & -\sin\theta\_{4} & 0 \\ \sin\theta\_{4} & 0 & \cos\theta\_{4} & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \;\_{5}^{4}T &= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d\_{5} \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned} \tag{1}$$

Therein, *i*−1 *i T* presents the homogeneous matrix of translation and rotation from the *i* − 1 joint coordinate system to the *i* joint coordinate system. The total transformation matrix of the end effector relative to the robot coordinate system is:

$$\begin{aligned} \,^0\_5 T = \,^0\_1 T^1\_2 T^2\_3 T^3\_4 T^4\_5 T = \begin{bmatrix} n\_x & o\_x & a\_x & p\_x \\ n\_y & o\_y & a\_y & p\_y \\ n\_z & o\_z & a\_z & p\_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{2} \end{aligned} \tag{2}$$

The end effector position equation is:

$$\begin{aligned} p\_x &= d\mathfrak{s}\sin\theta\_2 \sin(\theta\_3 + \theta\_4) - \sin\theta\_2 \cos\theta\_3 - 0.133\sin\theta\_2\\ p\_y &= -d\mathfrak{s}\cos\theta\_2 \sin(\theta\_3 + \theta\_4) + \cos\theta\_2 \cos\theta\_3 + 0.133\cos\theta\_2\\ p\_z &= -d\mathfrak{s}\cos(\theta\_3 + \theta\_4) - \sin\theta\_3 + d\_1 \end{aligned} \tag{3}$$

The end effector posture equation is:

$$\begin{aligned} p\_x &= d\_5 \sin \theta\_2 \sin(\theta\_3 + \theta\_4) - \sin \theta\_2 \cos \theta\_3 - 0.133 \sin \theta\_2 \\ p\_y &= -d\_5 \cos \theta\_2 \sin(\theta\_3 + \theta\_4) + \cos \theta\_2 \cos \theta\_3 + 0.133 \cos \theta\_2 \\ p\_z &= -d\_5 \cos(\theta\_3 + \theta\_4) - \sin \theta\_3 + d\_1 \end{aligned} \tag{4}$$

#### *2.3. Dynamic Model of Manipulator*

According to the analysis of manipulator joints [21], the five degrees of freedom of the manipulator are respectively the lifting base, waist rotation axis, arm joint, and wrist rotation. The lifting base only works when the length of the manipulator is insufficient, while the waist rotation and wrist rotation axis are tentatively defined as not playing a key role in the visual recognition and picking process; therefore, the manipulator is simplified as a two-joint manipulator with only a big arm and a small arm. Meanwhile, a dynamic model is established, as shown in Figure 5.

$$\mathcal{C}M(q)\ddot{q} + \mathcal{C}\{q,\dot{q}\}\dot{q} + \mathcal{G}(q) = \pi \tag{5}$$

**Figure 5.** Simplified schematic diagram of two-joint manipulator.

Herein, *q* = *q***1** *q***2** *T* , τ = [τ**1** τ**2**] *T*. In the formula, *q*, . *q*, .. *q* are respectively the joint angle position, velocity, and acceleration vectors of the manipulator. *M*(*q*) ∈ *Rn*×*n* is a positive definite mass inertia matrix. *C q*, . *q* ∈ *Rn*×*n* represents the centrifugal force and Coriolis force. *G*(*q*) ∈ *Rn* is gravity vector. τ ∈ *Rn* is the control moment input from each part of the joint. τd∈*Rn* is the unknown external disturbances. Therein, Formula (1) satisfies the following characteristics:

(1) Positive definite symmetry: For any *q* matrix, *M*(*q*) is a positive definite symmetric. . .

(2) Boundedness: Matrix functions *M*(*q*) and *C q*, *q* are uniformly bounded for all *q*, *q*.

(3) Skew symmetry: The matrix function *M*(*q*) − 2*C q*, . *q* is a skew symmetric matrix, i.e., for any vector α, there is α*T M*(*q*) − 2*C q*, . *q* α = 0.

#### **3. Visual Servo Control**

The visual servo control structure can be divided into position-based control and image-based control according to different feedback signals. The feedback signal of position-based control is defined in coordinate form in three-dimensional task space. Its basic principle is to estimate the target in a three-dimensional Cartesian coordinate system by extracting the image features and combining the geometric model of the known target, and then planning the trajectory and calculating the control quantity according to the position comparison between the end effector and the target of the manipulator. For the image-based control, the control quantity is calculated directly from the error signal in the image, and the task is completed by driving the manipulator [22]. In this paper, the image-based control method is used to output control signals after image calculation, and the sliding-mode control parameters are adaptively adjusted by the fuzzy neural network algorithm so as to improve the robustness and stability of the manipulator grasping.

#### *3.1. Visual Target Location*

According to the visual servo control system working process of fruit picking robots, the relationship between the position information of the robot and its joints should be determined first. The mapping relationship between the image plane coordinates and the three-dimensional coordinates in the real space can be established by the perspective model of the small hole so that the coordinate position of the target can be calculated [23]. Due to the vision camera adopted in this paper being the target localization of the monocular robot, its camera is installed at the end of the manipulator. According to the principle of the optical imaging of the camera, the model of monocular vision localization is established as shown in Figure 6, in which the mapping relation of two-dimensional image coordinates and coordinates in three-dimensional space is established by the transformed geometric coordinates.

**Figure 6.** Visual model of camera imaging.

Here, we assume that the target fruit coordinate in the coordinate system of the manipulator is (*Xa*, *Ya*, *Za*) and its coordinate in the camera coordinate system is (*Xc*, *Yc*, *Zc*). The relationship between the 3D coordinates (*Xc*, *Yc*, *Zc*) of the target in the camera coordinate system and its two-dimensional (2D) coordinates (*X*1, *Y*1) on the camera imaging plane is obtained.

$$\begin{cases} X\_1 = \frac{X\_\xi f}{Z\_\xi} \\ \ Y\_1 = \frac{Y\_\xi f}{Z\_\xi} \end{cases} \tag{6}$$

In the formula, *f* is the camera focal length. The coordinate transformation formula between the camera coordinate system and manipulator coordinate system can be obtained by the geometric analytic method shown in Figure 2.

$$\begin{cases} X = \sqrt{\frac{l\_2^2 + l\_3^2 - 2L\_2 l\_3 \cos(\pi - \theta\_3) - \left(L\_2 \sin \theta\_2 + L\_3 \sin \left(\theta\_2 + \theta\_3\right)\right)^2}{1 + \tan^2 \theta\_1}} \\\ Y = X \tan \theta\_1 \\\ Z = L\_1 + L\_2 \sin \theta\_2 + L\_3 \sin \left(\theta\_2 + \theta\_3\right) \end{cases} \tag{7}$$

In the formula, *L*1—Waist length

*L*2—Big arm length *L*3—Small arm length θ1—one DOF joint angle of manipulator θ2—two DOF joint angle of manipulator θ3—three DOF joint angle of manipulator

From Formula (3), the relationship between the target fruit coordinate in the manipulator coordinate system (*Xa*, *Ya*, *Za*), and the fruit coordinate in the camera coordinate system (*Xc*, *Yc*, *Zc*) can be obtained:

$$\begin{cases} X\_c = X\_d - X \\ Y\_c = Y\_d - Y \\ Z\_c = Z\_d - Z \end{cases} \tag{8}$$

The relationship between the robot angle change of each joint and the plane coordinate change of the target fruit imaging is obtained.

#### *3.2. Visual Servo Control*

Since the fruit picking robot works in unknown and uncertain environments and the visual servo control system has the characteristics of time-varying, strong coupling, and non-linearity, therefore, the visual servo control system based on images is used due to its advantages regarding insensitivity to errors. The structure of the image-based visual servo system is shown in Figure 7.

**Figure 7.** Image-based visual servo system structure.

In this system, the picking robot captures the target image with a camera installed by eye-in-hand mode, and the position information of the target fruit is obtained by an image processing system. The angle of each joint of the robot can be obtained by coordinate transformation. In order to improve the motion performance of the manipulator, a visual servo algorithm based on fuzzy neural network adaptive sliding mode control is adopted to overcome the uncertainty of the system so as to improve the robustness of the servo control of the manipulator.

#### **4. Complex Fuzzy Neural Network Sliding Mode Control**

#### *4.1. Model Design of Sliding Mode Algorithms*

Sliding-mode [24] variable structure control mainly uses a high-speed control rate under non-linear conditions. It can reach the predetermined sliding mode control track in a short time and move along the track according to the variable dynamic control system. The sliding mode control scheme is shown in Figure 8.

**Figure 8.** Structural diagram of the sliding-mode control (SMC) controller.

First, we set the ideal angle instruction to *qd*(*t*), and track the error of the sliding-mode controller:

$$
\dot{e}(t) = q(t) - q\_d(t) \tag{9}
$$

Then, we design the sliding surface as:

$$s(t) = \ddot{\mathbf{e}} + \Lambda\_1 \dot{\mathbf{e}} + \Lambda\_2 \mathbf{e} \tag{10}$$

Therein, **Λ** = *diag*(λ*i***1**, λ*i***2**, ... , λ*in*), λ*ij* > **0**, *i* = **1**, **2**, *j* = **1**, **2**, ... , *n*. Then, we set the Lyapunov function to:

$$V = \frac{1}{2} \mathbf{s}^T \mathbf{M} \mathbf{s} \tag{11}$$

Therefore: .

$$\dot{V} = \frac{1}{2}\dot{\mathbf{s}}^T \mathbf{M} \mathbf{s} + \frac{1}{2}\mathbf{s}^T \dot{\mathbf{M}} \mathbf{s} + \frac{1}{2}\mathbf{s}^T \mathbf{M} \dot{\mathbf{s}} \tag{12}$$

Bring the characteristics of the manipulator dynamics equation into the formula:

$$\dot{V} = \mathbf{s}^T M \dot{\mathbf{s}} + \mathbf{s}^T \mathbf{C} \dot{\mathbf{s}} = \mathbf{s}^T \left( M \left( \dddot{\overline{\boldsymbol{\eta}}} - \dddot{\boldsymbol{\eta}}\_d + \Lambda\_1 \ddot{\boldsymbol{\varepsilon}} + \Lambda\_2 \dot{\boldsymbol{\varepsilon}} \right) + \mathbf{C} \left( \ddot{\overline{\boldsymbol{\varepsilon}}} + \Lambda\_1 \dot{\boldsymbol{\varepsilon}} + \Lambda\_2 \boldsymbol{\varepsilon} \right) \right) \tag{13}$$

According to:

$$
\pi + \Lambda \pi = \Lambda \mathcal{U} \mathcal{U}\_{\text{SMC}} \tag{14}
$$

Find: .

$$\dot{V} = \mathbf{s}^T (H + \Lambda U I\_{\rm SMC}) \tag{15}$$

Therein:

$$H = M\left(\Lambda\_1 \ddot{e} + \Lambda\_2 \dot{e} - \dddot{\overline{q}}\_d\right) + \mathbb{C}\left(\Lambda\_1 \dot{e} + \Lambda\_2 e - \dddot{\overline{q}}\_d\right) - \left(\dot{M} + \Lambda \Lambda \right) \ddot{q} \tag{16}$$

$$- \left(\dot{\mathbb{C}} + \Lambda \mathbb{C}\right) \dot{q} - \left(\dot{\mathbb{C}} + \Lambda \mathbb{G}\right)$$

Then, we design the SMC system as:

$$
\hbar L\_{\rm SMC} = -\Lambda^{-1} \left( H + \eta \text{sgn}(s) \right) \tag{17}
$$

Find: .

$$\dot{V} = -\eta \mathbf{s}^T \text{sgn}(\mathbf{s}) = -\eta \|\mathbf{s}\| \le 0 \tag{18}$$

According to the LaSalle invariant set theorem, when *t* → ∞, *s* → 0, then *q* → 0, . *q* → 0.

#### *4.2. Sliding-Mode Control Based on Complex Fuzzy Neural Network*

The fuzzy neural network is a kind of special neural network that is a hybrid intelligent system formed by the combination of neural network and fuzzy logic. It combines the two kinds of techniques by combining the human-like reasoning of fuzzy systems with the learning and connection structure of neural networks [25,26]. In a nutshell, the fuzzy neural network (FNN) assigns a conventional neural network to fuzzy input signals and fuzzy weights. Its function is to use the fuzzy neural network structure to implement fuzzy logic reasoning.

In order to improve the control accuracy of the two-joint manipulator, a fuzzy neural network control algorithm, the fuzzy neural network (FNN) is constructed to optimize the conventional sliding mode control because of the chattering phenomenon in the conventional sliding-mode control [27–30]. The system consists of an input layer, an adaptive fuzzy rule layer, a rule layer, and an output layer. The structure of the system is shown in Figure 9. The input is a sliding surface, and the output is a control quantity. The following is the main flow chart of the FNN algorithm:

**Figure 9.** Structure of fuzzy neural network system.

Step 1: Input the layer data initialization of the fuzzy neural network, and transfer the sliding-mode vector variable *si*(*i* = 1, 2, ... , *n*) to the next layer.

Step 2: The nodes in each layer represent the membership functions of the members, which use Gauss function as an input:

$$\mu\_{i}^{j}(s\_{i}) = \exp\left[-\frac{\left(s\_{i} - \mathbf{x}\_{i}^{j}\right)^{2}}{\left(\sigma\_{i}^{j}\right)^{2}}\right], (i = 1, 2, \dots, n; \ j = 1, 2, \dots, m) \tag{19}$$

In the formula, *n* represents the number of input variables, and *m* is the number of member functions. *xj i* and σ*j i* represent the j level node of the i input from *si* to the Gauss function.

Step 3: The rule layer represents the fuzzy reasoning mechanism of the neural network, and the data results are obtained by multiplying the input signals of the layer. The formula is as follows:

$$\mathcal{g}\_k = \prod\_{i=1}^n \omega\_{ji}^k \mu\_i^j(s\_i) \tag{20}$$

Herein, *gk* represents the Kth output of the rule layer.

Step 4: The fourth layer is the output layer, in which each node represents an output variable. It integrates the output into the sum of all the input signals. The execution output of this layer deblurring is as follows:

$$y\_0 = \sum\_{k=1}^{N\_y} \omega\_k^0 g\_k \tag{21}$$

By adding FNN online cyclic training, inheriting the traditional sliding mode control system, and relaxing the requirement of system information, the SMC jitter problem can be eliminated. However, the traditional neural network training method mentioned above makes the learning speed of the algorithm slow and may fall into local minimum problem, etc. Therefore, a dropout layer is added on this basis to prevent over-fitting by randomly hiding layer nodes. The improved fuzzy neural network is expressed in vector form:

$$\mathbf{y} = \mathcal{U}\_{\text{NFNN}}(\mathbf{e}\_{\prime}\mathbf{x}\_{\prime}\sigma\_{\prime}\mathcal{W}\_{\prime}l\_{\prime}\mathbf{p}) = \mathcal{W}\mathbf{g} \tag{22}$$

In the formula, *W* is the weight matrix of the hidden layer and the output layer. *e* is the tracking error vector and the input vector of the system. *l* represents the fuzzy rules. *p* represents the dropout layer parameters of Bernoulli distribution.

Therefore, the equivalent control law of the optimal output estimation sliding surface is obtained.

$$\mathcal{U}L\_{\rm SMC} = \mathcal{U}^\*\_{\rm NN}(\varepsilon, x^\*, \sigma^\*, \mathcal{W}^\*, l^\*, p^\*) + \varepsilon = \mathcal{W}^\* \mathcal{g}^\* + \varepsilon \tag{23}$$

Herein, ε represents the error vectors of the network. *W*∗ , *x*∗ , σ∗ , *l* ∗ , *p*∗ are the optimal parameters of network. Then, the sliding mode control law of the improved fuzzy neural network can be expressed as:

$$\mathcal{U}\mathcal{U}(\mathbf{t}) = \mathcal{U}\_{\text{NN}}(\mathbf{e}, \mathbf{\hat{x}}, \mathbf{\hat{e}}, \mathbf{\hat{W}}, \mathbf{\hat{l}}, \mathbf{\hat{p}}) + \mathcal{U}\_{\text{r}} \tag{24}$$

Herein, *U*ˆ *NN* is the improved fuzzy neural network controller, which is used to estimate the sliding mode control law. *Ur* is an additional value of robustness, which ensures that the system would be on the sliding surface when t ≥ 0. ˆ*x*, σˆ,*W*ˆ , ˆ *l*, ˆ*p* represent the neural network parameter estimation.

$$
\overline{\mathcal{U}} = \mathcal{U}\_{\text{SMC}} - \mathcal{U} = \mathcal{W}^\* \mathcal{g}^\* + \varepsilon - \mathcal{W} \mathcal{\mathfrak{H}} - \mathcal{U}\_r = \overline{\mathcal{W}} \mathcal{g}^\* + \mathcal{W} \overline{\mathcal{g}} + \varepsilon - \mathcal{U}\_r \tag{25}
$$

In the formula, *W* = *W*∗ − *W*ˆ , *g* = *g*∗ − *g*ˆ, *g* can be expressed by Taylor expansion due to *g* being a non-linear activation function.

$$
\overline{\mathcal{g}} = \mathcal{g}\_{\overline{x}} \overline{\mathcal{e}} \overline{\mathcal{e}} + \mathcal{g}\_{\mathcal{o}} \overline{\mathcal{e}} \overline{\mathcal{e}} + o\_{\text{m}} \tag{26}
$$

Herein, *x* = *x*∗ − *x*ˆ, σ = σ∗ − σˆ can be expressed as:

$$\mathbf{g}^\* = \mathbf{\hat{g}} + \mathbf{g}\_x \mathbf{\overline{x}} \mathbf{e} + \mathbf{g}\_{\sigma} \mathbf{\overline{\sigma}} \mathbf{e} + o\_m \tag{27}$$

Bringing Formula (22) into Formula (20), then we find:

$$\begin{split} \overline{\mathcal{U}} &= \mathcal{W}^\* \mathcal{g}^\* + \varepsilon - \hat{\mathcal{W}} \hat{\mathcal{g}} - \mathcal{U}\_I \\ &= \mathcal{W}^\* (\mathcal{g} + \mathcal{g}\_x \overline{\mathcal{x}} e + \mathcal{g}\_\sigma \overline{\sigma} e + o\_m) + \varepsilon - \hat{\mathcal{W}} \mathcal{g} - \mathcal{U}\_I \\ &= \left( \mathcal{W}^\* - \hat{\mathcal{W}} \right) \mathcal{g} + \left( \overline{\mathcal{W}} + \hat{\mathcal{W}} \right) \mathbb{g}\_x \overline{\mathcal{x}} e + \left( \overline{\mathcal{W}} + \hat{\mathcal{W}} \right) \mathbb{g}\_\sigma \overline{\sigma} e + \varepsilon - \mathcal{U}\_I + \mathcal{W}^\* o\_m \\ &= \overline{\mathcal{W}} \mathcal{G} + \mathcal{W} \mathbb{g}\_x \overline{\mathcal{x}} e + \mathcal{W} \mathbb{g}\_\sigma \overline{\sigma} e - \mathcal{U}\_I + P \end{split} \tag{28}$$

Herein: *P* = *Wg x xe* + *Wg* σσ*e* + *W*∗ *om* + ε, ||*P*|| < *r*, *r* is a minimal positive number.

#### *4.3. Stability Analysis*

Next, we improve the fuzzy neural network sliding mode control algorithm according to the dynamics and kinematics equation of the manipulator to make the manipulator reach the prescribed sliding surface in the closed-loop control of the visual servo system, and reduce and eliminate the chattering problem of sliding-mode control, so as to ensure the long-term stability of the system.

Set:

$$
\hat{\mathcal{W}} = \eta\_1 \left( \mathbb{S}^{\mathcal{S}} \right)^T \tag{29}
$$

$$\pounds = \eta\_2 \Big(eS^T \mathcal{W} g\_x\big)^T \tag{30}$$

$$
\boldsymbol{\sigma} = \eta\_3 \Big( \boldsymbol{e} \boldsymbol{S}^T \boldsymbol{W} \mathbf{g}\_{\mathcal{S}} \Big)^T \tag{31}
$$

$$\mathcal{U}\_{\mathcal{I}} = \frac{\tau\_{0} \text{sgn}(S)}{\sum\_{i=1}^{n} S\_{i}^{2}} \tag{32}$$

Define the Lyapunov function as:

$$V = \frac{1}{2}S^T S + \frac{1}{2\eta\_1} tr\Big(\widetilde{W}\widetilde{W}^T\Big) + \frac{1}{2\eta\_2} tr\Big(\widetilde{\mathbf{x}\mathbf{x}^T}\Big) + \frac{1}{2\eta\_3} tr\Big(\widetilde{\sigma}\overline{\sigma}^T\Big) \tag{33}$$

Find the derivation:

$$=\frac{1}{2}S^{T}\dot{S}-\frac{1}{\eta\_{1}}\text{tr}\Big(\widetilde{\boldsymbol{W}}\widetilde{\boldsymbol{W}}^{T}\Big)-\frac{1}{\eta\_{2}}\text{tr}\Big(\widetilde{\boldsymbol{\text{xx}}}^{T}\Big)-\frac{1}{\eta\_{3}}\text{tr}\Big(\widetilde{\boldsymbol{\sigma}\boldsymbol{\sigma}}^{T}\Big)\tag{34}$$

Since . *S* = *U*, we introduce Formulas (6)–(15) and (6)–(17) and find:

$$\begin{array}{c} \dot{\mathsf{V}} = \mathsf{S}^{T} \Big[ \widetilde{\mathsf{W}} \mathsf{g} + \mathsf{W}\_{\mathcal{G}\mathbf{x}} \overline{\mathsf{x}} \mathsf{e} + \mathsf{W}\_{\mathcal{G}\mathbf{o}} \overline{\mathsf{o}} \mathsf{e} - \mathsf{U}\_{r} + P \Big] \\ - \frac{1}{\eta\_{1}} \mathsf{tr} \Big( \widetilde{\mathsf{W}} \mathsf{W}^{T} \Big) - \frac{1}{\eta\_{2}} \mathsf{tr} \Big( \overline{\mathsf{x}} \mathsf{x}^{T} \Big) - \frac{1}{\eta\_{3}} \mathsf{tr} \Big( \overline{\sigma} \sigma^{T} \Big) \\ = \mathsf{tr} \Big( \widetilde{\mathsf{W}} \Big( \mathcal{G} \mathcal{S}^{T} - \frac{1}{\eta\_{1}} \widetilde{\mathsf{W}}^{T} \Big) \Big) + \mathsf{tr} \Big( \mathsf{e} \mathcal{S}^{T} \dot{\mathcal{W}} \mathsf{g}\_{\mathcal{X}} - \frac{1}{\eta\_{2}} \overline{\mathsf{x}}^{T} \Big) + \mathsf{tr} \Big( \mathsf{e} \mathcal{S}^{T} \dot{\mathcal{W}} \mathsf{g}\_{\mathcal{o}} - \frac{1}{\eta\_{3}} \overline{\sigma}^{T} \Big) + \mathcal{S}^{T} \left( -\mathcal{U}\_{r} + P \right) \end{array} \tag{35}$$

According to Formulas (24), (27) and (30), we transform **. V** to:

$$\dot{\mathbf{V}} = \mathbf{S}^T(-\mathcal{U}\_I + P) \le \|\mathbf{S}^T\| \|\times\| - \mathcal{U}\_I + P\| = -\alpha \|\mathbf{S}^T\| \le 0 \tag{36}$$

Herein, α is a positive, sliding-mode control system based on the improved fuzzy neural network, and is asymptotically stable according to the Lyapunov stability principle. When *t* → ∞, s → 0.

#### **5. Simulation Analysis and Experiment**

The adaptive sliding-mode controller is simulated by matlab. The AC servo motor is used in the robot joint. The corresponding parameters are set according to the physical parameters of the picking robot arm as shown in Table 2.

$$H = \begin{bmatrix} \alpha + 2\varepsilon \cos(q\_2) + 2\eta \sin(q\_2) & \beta + \varepsilon \cos(q\_2) + \eta \sin(q\_2) \\ \beta + \varepsilon \cos(q\_2) + \eta \sin(q\_2) & \beta \end{bmatrix}$$

$$\mathcal{C} = \begin{bmatrix} (-2\varepsilon \sin(q\_2) + 2\eta \cos(q\_2))\dot{q}\_2 & (-\varepsilon \sin(q\_2) + \eta \cos(q\_2))\dot{q}\_2 \\ (\varepsilon \sin(q\_2) - \eta \cos(q\_2))\dot{q}\_1 & 0 \end{bmatrix}$$

$$\mathcal{G} = \begin{bmatrix} \varepsilon e\_2 \cos(q\_1 + q\_2) + \eta e\_2 \sin(q\_1 + q\_2) + (\alpha - \beta + e\_1)e\_1 \cos q\_1 \\ \varepsilon e\_2 \cos(q\_1 + q\_2) + \eta e\_2 \sin(q\_1 + q\_2) \end{bmatrix}$$

**Table 2.** Structure parameters of picking robot.

Herein, α, β, η, ε is constant. α = *I*1 + *m*1*l* 2 *c*1 + *Ie* + *mel* 2 *ce* + *mel* 2 1, β = *I*1 + *mel* 2 *ce*, ε = *mel*1*lcecos*(δ*e*), η = *mel*1*lcesin*(δ*e*). Take *a* = αβεη T , *a*ˆ is an estimation of *a*, set *a* = *a*ˆ − *a*, and *a* is constant, so . *a* ≈ . *a*ˆ.

Set the angle instruction of joint 2 as *qd*1 = *sin*(2π*t*), *qd*2 = *sin*(2π*t*), . *qd*1 and . *qd*2 are respectively the angular velocities of joints 1 and 2. Take **Λ** = 15 0 0 15 , **Λ**1 = 5 0 0 5 , **Λ**2 = 50 0 0 50 , *K*d = 100 0 0 100 . Set the gain of the adaptive fuzzy neural network to η1 = 20; η2 = 5; η3 = 2.

Firstly, the tracking curves of angular position and velocity of joints 1 and 2 are obtained by using sliding mode control without adding the fuzzy neural network algorithm, as shown in Figures 10–12.

From Figure 10, it can be seen that the angular position switching motion of common SMC joints without the fuzzy neural network is continuous and smooth. From Figure 12, it can be seen that joint 2 is more likely to be disturbed due to the need to deal with the conductive force from joint 1, so the output buffeting is relatively large.

The simulation results of a manipulator system with an improved fuzzy neural network sliding-mode control algorithm are shown in Figures 13–15. From Figure 15, it can be seen that compared with the traditional SMC scheme, the proposed fuzzy neural network sliding-mode control algorithm effectively eliminates the control chattering phenomenon. Meanwhile, the angular velocity tracking of joint 1 and joint 2 is more robust. It realizes high-precision control without detailed system information, and embodies the superiority of the improved fuzzy neural sliding-mode control algorithm.

**Figure 10.** Angle tracking of joints 1 and 2.

**Figure 11.** Angular velocity tracking of joints 1 and 2.

**Figure 12.** Joint 1 and 2 control inputs without neural network sliding mode.

In order to further verify the effectiveness of the sliding-mode algorithm of the fuzzy neural network system, the self-developed picking robot system is shown in Figure 16. The control system of the manipulator that is used in the experiment is an industrial computer with an Intel processor. The motion control system of the manipulator is connected with the host computer through serial communication, and the C control code generated by Matlab is imported into the control system.

The control instruction is generated according to the results of image recognition, so we process the control instructions by the sliding-mode algorithm of the fuzzy neural network and dynamic joint angle, output the dynamic joint angle, and bring into the kinematics equations related to the manipulator and output manipulator motor control instruction so as to control the manipulator gripper to reach the target position. Intensive experiments are performed on the apple-picking robot using the conventional PID algorithm, the sliding-mode control algorithm, and the improved fuzzy neural network sliding mode control on the robot arm servo control in the same experimental environment. Comparing the above three different algorithms on the picking-robot system on the grab speed and the grab accuracy, Table 3 is the data comparison using different algorithms.

**Figure 14.** Angular velocity tracking of joints 1 and 2.

**Figure 15.** Control input of joints 1 and 2 after adopting the intelligent control scheme.

**Figure 16.** (**a**) Representing the movement process of the robot arm of apple-picking robot; (**b**) Representing that the picking machine recognizes the target and controls the robot arm to grasp the target through the algorithm presented in this paper.

Through the comparison of experimental data, it can be known that the conventional PID algorithm captures a long time, while the sliding mode control algorithm can effectively reduce the grab time, but there is a lower catch success rate, and the improved fuzzy neural network algorithm is adopted. The sliding mode control algorithm not only improves the grabbing efficiency, but also greatly improves the success rate of the capture. It is shown in the result of the visual servo motion of the manipulator that the fruit-picking robot based on the improved fuzzy neural network sliding-mode algorithm has good stability and robustness. Figure 14 shows that the fruit-picking robot completes the visual servo control process of the manipulator using an improved algorithm.

#### **6. Conclusions**

In this paper, based on the self-developed apple-picking robot, the kinematics and dynamics equations of the robot are established. The image-based visual servo control method is adopted to control the manipulator in order to improve the grasping accuracy in the picking process. Then, the joint control performance of the control system has been improved by the adaptive fuzzy neural network sliding-mode control algorithm. Finally, the superiority of the algorithm is verified by simulation and experiment. The results show that the method improves the shortcomings of the common sliding mode control for the training control jitter in the actual robotic arm visual servo system to a certain extent, and improves the stability of the servo control.

Due to the low stability of the mechanical design of the current picking robot end effector, the existing mechanical structure of the picking robot will be improved in future work, and continue to verify the superiority of the current algorithm on the basis of the new terminal actuator, and complete the picking experiment in the real environment of the apple-picking robot.

**Author Contributions:** W.C. conceived and designed the experiments; W.C. performed the experiments; T.X. contributed reagents/materials/analyzed the data; T.X. and J.L. contributed reagents/materials/analysis tools; T.X. and M.W. wrote the paper; D.Z. analyzed the data.

**Funding:** This research is supported by the "Six talent peaks" high-level talents projection Jiangsu Province (2016-GDZB-021), Postgraduate Research & Practice Innovation Program of Jiangsu Province (KYCX18\_2332), Project funded by China Postdoctoral Science Foundation (2016M601691), Industrial Foresight Project of Zhenjiang City (GY2018018).

**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* **Optimized Combination of Spray Painting Trajectory on 3D Entities**

## **Wei Chen 1,2,\*, Xinxin Wang 1, Hao Liu 1, Yang Tang 3 and Junjie Liu 1**

Received: 27 November 2018; Accepted: 4 January 2019; Published: 9 January 2019

**Abstract:** In this research, a novel method of space spraying trajectory optimization is proposed for 3D entity spraying. According to the particularity of the three-dimensional entity, the finite range model is set up, and the 3D entity is patched by the surface modeling method based on FPAG (flat patch adjacency graph). After planning the spray path on each patch, the variance of the paint thickness of the discrete point and the ideal paint thickness is taken as the objective function and the trajectory on each patch is optimized. The improved GA (genetic algorithm), ACO (ant colony optimization), and PSO (particle swarm optimization) are used to solve the TTOI (tool trajectory optimal integration) problem. The practicability of the three algorithms is verified by simulation experiments. Finally, the trajectory optimization algorithm of the 3D entity spraying robot can improve the spraying efficiency.

**Keywords:** spray painting robot; FPAG; GA; ACO; PSO; TTOI problem

#### **1. Introduction**

With the development of the social economy and the improvement of life, people have higher requirements for product quality and product appearance. Moreover, some products with higher surface spraying quality, such as furniture, automobiles, and artworks, determine the quality of the product appearance and the competitiveness of products in the market. Therefore, people pay more and more attention to surface spraying technology [1]. The traditional spraying technology is that the spraying workers with spraying guns directly spray the workpiece to be sprayed. During spraying, the paint mist diffuses into the surrounding environment, which not only pollutes the environment, but also seriously harms the physical and mental health of spraying workers. To solve the problems caused by traditional spraying, automatic spraying systems have been developed. As a typical table of spraying automation equipment, a spraying robot has many advantages, such as good uniformity of the coating thickness, high repetitive positioning accuracy, wide applicability, and high efficiency. At the same time, spraying robots can free workers from a toxic, flammable, and explosive working environment [2].

In many fields of industrial production, the surface spraying of the workpiece is an essential step, and the types of workpiece modeling are becoming more and more abundant. During the spraying operation, the workpiece may have multiple spraying surfaces, and the curvature of each spraying surface is different. Usually, a three-dimensional (3D) entity modeling method is needed to form these parts, as shown in the 3D entity diagram in Figure 1. In recent years, due to the wide application of 3D solid modeling in various fields, scholars have made remarkable achievements in the research of 3D

solid modeling [3–7]. The spraying of 3D solid modeling has a certain novelty because the spraying is more interested in the surface shape of the workpiece. For the research of this paper, we use the finite spraying modeling technology and the surface modeling method based on the plane patch adjacency graph (FPAG) for modeling. It should be pointed out that the 3D entities mentioned in this paper are not the same as polyhedra. A polygon is a spatial geometry surrounded by several planar polygons, each of which is a plane. However, each face of a 3D entity can be a free-form curved surface. In this sense, the polyhedron is only a subset of the 3D entity.

**Figure 1.** (**a**) 3D entities with concave surfaces. (**b**) 3D entities with convex surfaces.

In addition, research on trajectory optimization for spray painting robots oriented to 3D entities is immature. Existing trajectory optimization methods can only be applied to 3D entities with convex surfaces, as shown in Figure 1b. For a 3D entity with concave surfaces (Figure 1a), the research on the trajectory optimization for the robot in this field is still a blank as the shape of the entity is complex and the robot is required to be extremely flexible in automatic spray painting operation. The trajectory optimization method for the spray painting robot introduced in this paper can only be applied to 3D entities with convex surfaces.

The idea of trajectory optimization for a spray painting robot oriented to a 3D entity is: First, a simple mathematical model of the paint deposition rate is established by the experimental method and the 3D entity is sliced by using the FPAG surface modeling method [8–10]. Secondly, after planning the painting path on each patch, the spray painting trajectory is optimized on each patch with the objective function of the variance of the paint thickness at discrete points and the ideal paint thickness. Finally, the spray painting trajectories on the individual patches are optimized, and the optimized trajectories of the spray painting robot on the 3D entities are formed eventually.

#### **2. The Establishment of the Mathematical Model**

The spraying mathematical model mainly includes the position and direction of the end-effector and the paint deposition rate model.

The establishment of the paint deposition rate model is an important problem in trajectory optimization for spray painting robots. Considering that the expressions of these models are rather complex, it is not applicable to build the mathematical model of the paint deposition rate on 3D entities. Therefore, in this paper, the specific mathematical expression of the finite range model takes the following factors into account: The distance and direction from the gun to the working surface, the curvature of the workpiece surface, and the angle of the paint gun (i.e., the cone angle corresponding to the paint flow).

Before building a finite-scope model, we made the following assumptions: The paint particles sprayed by the paint gun form a cone in space. Suppose the opening angle, ϕ, is half of the angle, *ϕ*, and the opening angle, *ϕ* < 90◦. The definition of the opening angle, *ϕ*, can be seen in Figure 2.

**Figure 2.** The finite range model.

Considering that the amount of paint to be obtained is smaller when the distance, *L*(*L* > 0), from the point, *s*(*x*, *y*, *z*) ∈ *S*, on the surface to the gun increases. Additionally, the amount of paint obtained at the points on the surface is also decreased when the angle, *θ*(*θ* < *ϕ*), is gradually increasing. The amount of the paint obtained at the point, *s*, on the workpiece surface can be expressed by the following equation:

$$\frac{c(\theta,\phi)}{L^2} \tag{1}$$

Among which:

$$\mathfrak{c}(\theta,\mathfrak{q}) \begin{cases} > 0, & \theta < \mathfrak{q} \\ = 0, & \theta \ge \mathfrak{q} \end{cases} \tag{2}$$

$$L = \sqrt{\left(x - \mathbf{p}\_x(t)\right)^2 + \left(y - \mathbf{p}\_y(t)\right)^2 + \left(z - \mathbf{p}\_z(t)\right)^2} \tag{3}$$

**p***x*, **p***y*, and **p***z* express, respectively, the coordinates of the gun on the X-axis, Y-axis, and Z-axis. Assuming that the workpiece surface is a curved surface, the paint deposition rate of a point, *s*(*x*, *y*, *z*) ∈ *S*, on the surface is proportional to the inner product of the following two vectors (shown in Figure 2): First, the unit normal vector, *n*(*s*), of the point. Second, the unit direction vector, *d*(*p*(*t*),*s*), of the gun and the point, *s*. The function, *d*(*p*(*t*),*s*), is defined as follows:

$$d(p(t),s) = \frac{(x - p\_x(t))i + \left(y - p\_y(t)\right)j + (z - p\_z(t))k}{L} \tag{4}$$

where, *i*, *j*, and *k* denote unit vectors in the positive direction of the *X*, *Y*, and *Z* axis, respectively.

Based on the above assumptions, we can derive the paint deposition rate function of a point, *s*, on the surface of the workpiece:

$$\stackrel{\bullet}{f}(s, p(t), t) = \left(\frac{c(\theta, q)}{\left(x - p\_x(t)\right)^2 + \left(y - p\_y(t)\right)^2 + \left(z - p\_z(t)\right)^2}\right) \cdot d(p(t), s) \cdot n(s) \tag{5}$$

The variable, *θ*, in Equation (5) is not significant. In the equation, *θ* is the angle of the gun and a point, *s*, on the workpiece surface and the axis of the gun. It is related to the coordinates of the point, *s*, on the workpiece surface, the position, *p*(*t*), and direction, *o*(*t*), of the paint gun, which can be defined as:

$$\theta = \cos^{-1}(d(\mathfrak{p}(t), \mathfrak{s}) \cdot \mathfrak{o}(t))\tag{6}$$

The selection of function, *c*(*θ*, *ϕ*), depends on the basic spray characteristics of the gun and some parameter settings, such as the air pressure of the gun and the velocity of the paint flow. In the usual case, the function, *c*(*θ*, *ϕ*), reaches its maximum when *θ* = 0 (that is, it just below the paint gun). When *θ* → *ϕ*, *c*(*θ*, *ϕ*) → 0. The following gives a concrete model of function, *c*(*θ*, *ϕ*):

$$\mathcal{L}(\theta, \mathfrak{p}) = \begin{cases} a \frac{\cos(\theta) - \cos(\mathfrak{p})}{\left(1 - \cos(\mathfrak{p})\right)^2} & \theta \le \mathfrak{p} \\ 0 & \text{otherwise} \end{cases} \tag{7}$$

The painting velocity of the paint gun and air pressure can be adjusted by changing the values of the parameters, *α* and *ϕ*, in the formula above, respectively. It can also be seen from the formula above that the values of the parameters, *α* and *ϕ*, are related to the maximum of the function, *c*(*θ*, *ϕ*).

The finite-range model here is circular on the plane corresponding to the paint distribution model. When the workpiece surface has a certain curvature, the paint distribution should become oval. For surfaces with small curvatures, the paint distribution, which is actually elliptical, can be approximated as a circular. In addition, the paint deposition rate model also illustrates that the total amount of paint sprayed to the workpiece surface (the total amount of paint sprayed from the gun) has nothing to do with the surface shape of the workpiece and the distance from the gun to the workpiece surface, which is in line with the actual situation.

#### **3. Segmentation for 3D Entity**

To obtain the optimal trajectory on the 3D solid surface, the first step is to model the workpiece surface. Second, due to the particularity of the spraying surface, trajectory optimization of the spraying robot is relatively difficult in a practical application. To obtain a good spraying effect, the finite range model modeling method is adopted. The third step is to use the FPAG surface modeling method to simplify the plane patch adjacency graph. As shown in Figure 3, the specific steps are as follows: (1) Divide the triangular grid of the 3D solid surface. (2) Set the maximum normal vector threshold and connect the triangle surface into a smaller flat area according to the triangle connection algorithm. (3) Each patch is approximately planar, and at least one side of each patch is part of the 3D solid ridge.

**Figure 3.** Step diagram of the surface modeling method based on the flat patch adjacency graph (FPAG).

#### **4. Trajectory Optimization on Each Patch**

The geometric properties of 3D entity surfaces are more complex compared with two-dimensional planar surfaces and regular curved surfaces. Therefore, to simplify the problem, we will describe a relatively simple and practical trajectory optimization method in the following. This method is fast and the process is simple. It can meet the actual needs completely.

The trajectory of a spray painting robot mainly consists of two factors: Path and velocity. In the spray painting process, the painting path can be obtained by determining the width of the overlapping areas formed by two paint strokes. Therefore, to determine the trajectory of a spray painting robot, we only need to determine the velocity of the paint gun and the width of the overlapping area formed by two paint strokes. *x* represents the distance from a point, *s*, to the first path in the painting radius, *d* represents the width of the overlapping area formed by two paint strokes, *R* represents the distance from the surface point to the spray direction, *o*(*t*), O is the TCP (tool center point). Then, the paint thickness at point, *s*, is:

$$q\_3(\mathbf{x}) = \begin{cases} \, \, lq\_1(\mathbf{x}) & 0 \le \mathbf{x} \le R - d \\\, q\_1(\mathbf{x}) + q\_2(\mathbf{x}) & R - d < \mathbf{x} \le R \\\, q\_2(\mathbf{x}) & R < \mathbf{x} \le 2R - d \end{cases} \tag{8}$$

*q*1(*x*) and *q*2(*x*) are the paint thickness at point *s* when spray painting on two adjacent paths, respectively. The formulas of *q*1(*x*) and *q*2(*x*) are:

$$q\_1(\mathbf{x}) = 2 \int\_0^{t\_1} f(r\_1) dt,\ 0 \le x \le R \tag{9}$$

$$q\_2(\mathbf{x}) = 2 \int\_0^{t\_2} f(r\_2) dt,\ \mathcal{R} - d \le \mathbf{x} \le 2\mathcal{R} - d \tag{10}$$

among which, *t*1 = √*R*2 − *x*2/*v*, *t*2 = *R*2 − (2*R* − *d* − *x*) 2 /*v*, *r*1 = (*vt*) 2 + *x*2, *r*2 = (*vt*) 2 + (2*R* − *d* − *x*) 2 . *t*1 and *t*2 represent half of the spray painting time that the gun paints in two adjacent painting paths at point *s*, respectively. *r*1 and *r*2 represent the distance from point *s* to the central projection point of TCP in two adjacent painting paths, respectively, *t* is the time that the gun moves from point *O* to point *s'*. *s'* is the projection of the point, *s*, on the painting path. The following expression can be obtained from (9) and (10):

$$q\_s(\mathbf{x}, d, v) = \frac{1}{v} f(\mathbf{x}, d) \tag{11}$$

where, *J* is a function of *x* and *d*. To make the paint thickness on the surface as uniform as possible, the difference between the actual paint thickness and the variance of ideal paint thickness at point *s* are taken as the optimization objective function:

$$\min\_{d \in [0,R], \upsilon} E\_1(d, \upsilon) = \int\_0^{2R - d} (q\_d - q\_\sharp(\mathbf{x}, d, \upsilon))^2 d\mathbf{x} \tag{12}$$

In the equation above, *qd* is the ideal paint thickness. Since the maximum paint thickness, *q*max, and the minimum paint thickness, *q*min, determine the uniformity of the paint thickness on the workpiece surface, and *q*max and *q*min also need to be optimized:

$$\min\_{d \in [0, R], v} E\_2(d, v) = (q\_{\max} - q\_d)^2 + (q\_d - q\_{\min})^2 \tag{13}$$

It can be obtained from Equations (11)–(13) that:

$$\min\_{d \in [0, R], \upsilon} E(d, \upsilon) = \frac{1}{2R - d} E\_1(d, \upsilon) + E\_2(d, \upsilon) \tag{14}$$

Additionally, from Equations (9) and (10), the expressions of the maximum paint thickness and minimum paint thickness can be described as:

$$q\_{\text{max}} = \frac{1}{\upsilon} f\_{\text{max}}(d) \tag{15}$$

$$q\_{\rm min} = \frac{1}{v} f\_{\rm min}(d) \tag{16}$$

Let *∂E*(*d*,*v*) *∂v* = 0, it can be obtained from Equations (11), (14)–(16) that:

$$v = \frac{\frac{1}{2R - d} \int\_0^{2R - d} f^2(x, d) dx - f\_{\text{max}}^2(d) - f\_{\text{min}}^2(d)}{q\_d \left[ \frac{1}{2R - d} \int\_0^{2R - d} f(x, d) dx + f\_{\text{max}}(d) + f\_{\text{min}}(d) \right]} \tag{17}$$

It can be seen that the spray painting rate, *v*, can be expressed as a function of the width, *d*, of the paint overlapping area formed by two painting strokes. Therefore, the minimum value of *E*(*d*, *v*) is only related to *d*. The golden section method [11] can be used to obtain the optimization value, *d*, so that the optimized trajectory on each patch can be obtained too.

#### **5. Tool Trajectory Optimal Integration on 3D Entity**

After the track optimization of each patch, the optimal combination of trajectories connecting each patch should also be considered to speed up the spraying speed of the spraying robot. The first step is to transform and model the TTOI problem. TTOI problem is represented by the Hamiltonian diagram. In the second step, the corresponding optimization algorithm is used to solve the TTOI problem. The third step, through simulation and spray painting experiments, verification, and comparison, identifies the advantages and effectiveness of the algorithm.

#### *5.1. The Transformation and Modeling of Tool Trajectory Optimal Integration*

As is shown in the Figure 4, the TTOI (tool trajectory optimal integration) on each patch after the 3D entity segmentation is expressed [12]. To make the problem less complicated, the trajectories are considered as an edge. The ultimate purpose of the TTOI problem is to spray patches on the workpiece surface to make the spraying path of the robot the shortest. According to graph theory, a non-directional connection graph, G, is assumed (*V*, *E*, *R*, *ω*: *E*→*Z+*), among which *V* denotes the vertex set, *E* denotes the edge set, *R* denotes any subset of *E*, and *ω* denotes the weight of the edge (the length of the actual spray path). The problem of TTOI is to find a path passing all edges only once with the shortest distance in graph, G. Similar to the traveling salesman problem (TSP), which is a common problem in the optimization problem, the TTOI problem is also a typical NP (non-polynomial) problem.

**Figure 4.** Tool trajectory optimal integration on each patch.

Suppose that D = {*dij*} (*i, j* = 1, 2, ... , *n*), the shortest distance between vertex *i* and vertex *j*, which are not on the same edge in graph G, the distance between the vertices can be calculated according to the Floyd algorithm. To make the problem less complicated, the TTOI problem can be expressed by the Hamiltonian method. As shown in Figure 5, a vertex is used to represent an edge of the original graph G to form a complete Hamiltonian [12]: *g* (*VH*, *EH*, *ωH*), among which *VH* denotes the vertex set, *EH* denotes the edge set, and *ωH* denotes the weight of the edge and *ωH*∈D. In the graph, *g*, the weight of each edge is not fixed. Its value is determined by the order of vertices on the same edge in the original graph, G. Suppose that the order of the vertices set, *VH* = {*v*1, *v*2 ... ... *vn*}, in graph g is *T* = (*t*1, *t*2 ... ... *tn*) *ti*∈*VH* (*i* = 1, 2, . . . , *n*), and the TTOI problem can be defined as Equation (18):

$$\text{minim}\overline{L} = \sum\_{i=1}^{n} \omega\_{i} + \sum\_{j=1}^{n-1} \omega\_{j}^{\text{H}} \tag{18}$$

where *ωi* is the weight of the edges in the primitive graph, G, corresponding to vertices, *t*1, *t*2 ... ... *tn*, in graph *g* and *ωH j* denotes the weight of the edge in graph *g*. Since the weight, *ωi*, of each edge in the original graph, G, is considered to be a fixed value in this problem, the above optimization problem can be reduced to Equation (12):

$$\text{minim}L = \sum\_{j=1}^{n-1} \omega\_j^{\mathbb{H}} \tag{19}$$

**Figure 5.** Transformation of the original graph, *G*, into the Hamiltonian graph, *g*.

The spraying robot is the most complex one in the control of the industrial robot because of its many parameters. Especially, the trajectory optimization of the spraying robot on the complex surface makes the actual operation difficult. Therefore, finding the arrangement of all vertices in the Hamilton diagram makes the path, L, of the painting robot the shortest, which becomes a TTOI problem. Because of the large number of parameters, spray robots are the most complex control of industrial robots. Especially, the trajectory optimization of the spraying robot on the complex surface makes the actual operation difficult. Therefore, finding the arrangement of all vertices in the Hamilton diagram makes the shortest path, L, of the painting robot a TTOI problem. To solve the TTOI problem, the improved genetic algorithm, ant colony algorithm, and particle swarm optimization (PSO) proposed in this paper can be used to optimize the trajectory of a spraying robot on complex surfaces. In the process of fragmentation, these intelligent algorithms can solve the trajectory optimization problem between patches. For the first time, these intelligent algorithms have been used for spraying complex surfaces. The previous links between patches are random combinations. Finally, the advantages and disadvantages of each algorithm are illustrated by experiments.

#### *5.2. Solving the TTOI Problem with the Genetic Algorithm*

Genetic algorithm (GA) is a method to search for the optimal solution by simulating the natural evolution process. Therefore, this algorithm has good effects on the NP (non-polynomial) problem in combinatorial optimization and can be used to solve the TTOI problem. According to the particularity of TTOI [13–15], GA needs a special individual code and crossover, mutation, and other genetic manipulation methods.

**(1) Individual code:** The length of the individual code is |*VH*|. Since each vertex in the Hamilton graph represents one edge of the original graph, G, to distinguish the start and end points of each edge. The individual code contains the binary code, *Psi,* representing the direction of each edge in the original graph, G.

**(2) Fitness function:** The values of fitness function are used to determine which individuals can enter the next round of evolution and which individuals need to be removed from the population. To facilitate the selection operation in the genetic algorithm, the optimization of the minimum value is usually converted to optimization of the maximum value, and the fitness function can be taken as: *F* = *U* − *L*, where *U* should be selected as an appropriate number, to make the fitness of all individuals positive. In the process of population evolution, to select the individuals with high fitness, the population size is maintained as the value, *Psize*. According to the fitness function rule, the *Psize* individuals with the highest fitness are passed to the next generation.

**(3) Crossover:** Crossover is the process of exchanging the partial codes between two individuals with a certain probability to generate new individuals. Here, order crossover (OX) is used on *Pi* while two-point crossover is used on *Psi*. OX ensures that the original order of each vertex is almost the same when the effective sequence of the individual itinerary is modified [16]. The main idea of OX is: A conventional two-point crossover is performed, followed by an effective sequence modification of the individual itinerary. When modifying, the original relative access order of each point should be maintained as much as possible. Basic steps of OX are as follows:

(a) In the individual code strings, *Px* and *Py*, representing the spray painting order, the positions after the two loci, *i* and *j*, are randomly selected as the intersection. That is, each locus between the (*i* + 1)-th locus and the *j*-th locus is defined as an intersection area, and the contents of the intersection region are respectively memorized to *Wx* and *Wy*.

(b) According to the mapping relation in the intersection area, find all *Pxq* − *Pxq* (*p* = *i* + 1, *i* + 2, ... , *j*) loci *q* in the individual *Px* and set them as vacancies. Find all *Pxq* − *Pxq* (*p* = *i* + 1, *i* + 2, ... , *j*) loci, *r*, in the individual, *Py*, and set them as vacancies.

(c) The individuals, *Px*, *Py*, are left-shifted circularly until the first vacancy in the code string is moved to the left end of the intersection area. Then, all the vacancies are concentrated in the intersection area, and the original gene values in the intersection area are sequentially moved backward.

(d) Exchange the content in *Wx* and *Wy* and put them into the intersection area of individual *Px*, *Py*. The result is a new spray painting order.

**(4) Mutation operation:** *Pi* is subjected to inversion mutation to generate a new individual. A basic variation is applied to *Psi*, where one or more loci are randomly selected for individual code and the gene values of these loci are inverted.

Thus, the genetic algorithm of the TTOI problem is shown in Figure 6.

**Figure 6.** Flow chart of the GA (Genetic algorithm) of the TTOI (tool trajectory optimal integration) problem.

Taking 3D entities as spray objects, simulation experiments were carried out using genetic algorithm programming to verify the effectiveness of the TTOI problem. According to the segmentation method of the 3D entity, a 3D entity is divided into seven patches; that is, the individual code, *Pi* and *Psi*, are seven bits in the genetic algorithm. The parameters of the algorithm are as follows: Population size, *Psize* = 100; crossover probability, *xrate* = 0.20; mutation probability, *mrate* = 0.05; and the maximum number of evolutionary generation, *T* = 100. The corresponding evolutionary processes of different solutions are shown in Figure 7. As can be seen from the figure, the value of the objective function of the optimal individual decreases monotonously with the evolution process and eventually tends to be constant. After about 70 generations of evolution, the average fitness remains stable and the algorithm converges.

**Figure 7.** The simulation result of solving the TTOI problem with GA.

#### *5.3. Solving the TTOI Problem with the Ant Colony Algorithm*

Ant colony optimization (ACO) is a probabilistic intelligent algorithm for finding the optimal path. It originated from the behavior of ants to find the path in the process of searching for food. It has strong anti-interference ability, strong compatibility, and other characteristics. The algorithm initializes the following individual information: Not visited vertices (NVV), not visited edges (NVE), visited edges (VE), and tour length (TL). Through the memory function of the population, individual information is constantly updated and adjusted. Taking the connection graph, G, shown in Figure 5 as an example, if the algorithm starts when the ant is at the vertex, 1, the initialization information is:

$$\begin{array}{c} \text{NVV [1] = \{1, 2, 3, 4, 5, 6, 7, 8, 9, 10\}.}\\ \text{VV [1] = \{\}.}\\ \text{NVE [1] = \{ (1, 2), (3, 4), (5, 6), (7, 8), (9, 10) \}.}\\ \text{VE [1] = \{\}.}\\ \text{TL [1] = 0.0} \end{array}$$

After time, Δ*t*, the pheromone on trajectory (*i*, *j*) *i* adjusted as follows:

$$
\tau\_{ij}(t + \Delta t) = \rho \tau\_{ij}(t) + \Delta \tau\_{ij} \tag{20}
$$

where *ρ* represents the volatilization rate of pheromone, *τij*(*t*) represents the accumulation amount of pheromone on the track (*i*, *j*) at time *t*, Δ*τij* represents the increment of the pheromone on the trajectory (*i*, *j*) after the time, Δ*t*, which can be calculated as follows:

$$
\Delta \tau\_{ij} = \sum\_{k=1}^{m} \Delta \tau\_{ij}^{k} \tag{21}
$$

Δ*τk ij* denotes the pheromone on the trajectory (*i*, *j*) during the searching process of the *k*-th ant, the expression of which is:

$$
\Delta \mathbf{r}\_{ij}^k = \begin{cases}
\frac{Q}{TL\langle k\rangle} & \text{ant } k \text{ pass path } (i, j) \\
0, & \text{else}
\end{cases}
\tag{22}
$$

Among which, *Q* is a constant. The pheromones on each trajectory during initialization are: Δ*τij* = 0. At the time, *t*, the transition probability of an ant, *k*, from vertex *x* to other feasible vertices is:

$$p\_{ij}^k = \begin{cases} \frac{(\tau\_{ij}(t))^a (\eta\_{ij}(t))^\beta}{\sum\_{s \in already\_k} (\tau\_{is}(t))^a (\eta\_{is}(t))^\beta}, & j \in allowed\_k\\ 0, & otherwise \end{cases} \tag{23}$$

where *ηij* denotes the visibility on track (*i*, *j*), which reflects the degree of heuristic from vertex *i* to vertex *j*. Here, let *ηij* = 1/*dij*, *dij* is the distance from vertex *i* to vertex *j*. The parameters, *α* and *β*, denote the influence weights of *τij*(*t*) and *ηij*(*t*) on the whole transition probability. *alowedk* denotes the feasible neighborhood of the ant k at vertex i (end of the edge in the list NVE). Thus, the ant colony algorithm of the TTOI problem is shown in Figure 8.

**Figure 8.** Flow chart of the ACO of the TTOI problem.

In the following, the validity of using ACO to solve the TTOI problem is verified by simulation experiments. Assuming that a 3D entity workpiece is divided into five patches, the number of edges in the connected graph, G, is five and the number of vertices is *m* = 10. The parameters of the algorithm are chosen as follows: *α* = 1, *β* = 5, *ρ* = 0.5, *Q* = 100 and the maximum number of cycles is *Nmax* = 100. Figure 9 shows the evolution of the optimal solution obtained from the algorithm. It can be seen from the figure that the length of the spray painting trajectory decreases monotonically with the evolutionary process. After about 70 generations of evolution, the length of the trajectory no longer changes and the algorithm converges.

**Figure 9.** Simulation result of the ACO algorithm.

#### *5.4. Solving the TTOI Problem with Particle Swarm Optimization*

Particle swarm optimization (PSO) is easy to implement and there is no need to adjust a lot of parameters compared with other optimization algorithms. Also, it does not need gradient information, which is an effective tool to solve the optimal combination [17–19]. In the algorithm, each individual is a particle, and each particle represents a potential solution. Assuming that *zi* = (*zi*1, *zi*2, ..., *ziD*) is the D-dimensional position vector of the i-th particle, the current fitness value of *zi* can be calculated according to the fitness function so that the position of the particle can be measured. The process of calculating the minimum length of the spray path length can be selected as the fitness function in the TTOI problem. *vi* = (*vi*1, *vi*2, ..., *viD*) is the flying speed of particle i; that is, the distance of particle movement. *pi* = (*pi*1, *pi*2, ..., *piD*) is the optimal position of the particle to date. *pg* = *pg*1, *pg*2, ..., *pgD* is the optimal position of the particle swarm to date. In each iteration, the velocity and position of the particles can be updated according to:

$$
\sigma\_{\rm id}^{k+1} = \sigma\_{\rm id}^k + c\_1 r\_1 (p\_{\rm id} - z\_{\rm id}^k) + c\_2 r\_2 (p\_{\rm gd} - z\_{\rm id}^k) \tag{24}
$$

$$z\_{id}^{k+1} = z\_{id}^k + v\_{id}^{k+1} \tag{25}$$

where *i* = 1, 2, ... , *m*, *d* = 1, 2 ... *D*, *r*1 and *r*2 are random numbers between [0, 1], and *c*1 and *c*2 are learning factors. Thus, the particle swarm algorithm of the TTOI problem is shown in the Figure 10.

Assuming that the 3D entity artifact is divided into five patches, the number of edges in the connection graph, G, shown in Figure 5 is five and the number of vertices is *m* = 10. To guarantee the precision of the algorithm, the maximum number of cycles is *Nmax* = 100. To ensure that the particle does not skip the optimal solution and can search the search space sufficiently, let *ε* = 1000. To ensure accuracy and reduce the amount of calculation, take the number of particles as 20. The learning factors, *c*1 and *c*2, can make the particles have self-summary and the ability to learn from the outstanding individuals in the group, to be close to the best in their own history and the history within the group. These two parameters have little effect on the convergence of the algorithm, but if we adjust these two parameters properly, we can reach the convergence faster. After adjusting the values of *c*1 and

*c*2 several times and analyzing the effect of *c*1 and *c*2 on the optimal fitness, we can conclude that *c*1 = *c*2 = 2 is a better choice for the TTOI problem. Figure 11 shows the evolution of the optimal solution obtained from the algorithm. It can be seen from the figure that the length of the spray painting trajectory decreases monotonously with the evolutionary process, and finally tends to a definite value. As the spray painting robot does not consider the obstacle avoidance during the working process, the environment information is known and is relatively simple, so the particle swarm algorithm converges faster. It can be seen from Figure 11 that after about 80 generations of evolution, the length of the spray painting trajectory does not change and the algorithm converges.

**Figure 10.** Flow chart of the PSO of the TTOI problem.

**Figure 11.** Simulation result of the PSO algorithm.

#### *5.5. Comparison of Algorithms*

In this paper, according to the built coating accumulation model: The finite range model, then FPAG (flat patch adjacency graph) is used to treat spraying workpiece, and the workpiece surface is divided into numerous patches. In each patch, the trajectory optimization algorithm in part 3 is used to obtain the trajectory. Finally, the genetic algorithm, ant colony algorithm, and particle swarm optimization algorithm are respectively used to obtain the optimal path to connect all patches to complete the spraying work. Assuming the ideal paint thickness of *qd* = 50 μm, the maximum allowable deviation of paint thickness of *qw* = 10 μm, the bottom radius of the cone paint sprayed by the gun is *R* = 60 mm. The cumulative rate of the paint is obtained from the spray test data on the plate [12]:

$$f(r) = \frac{1}{15}(R^2 - r^2)\mu\text{m/s} \tag{26}$$

After generating and optimizing the spray painting trajectories on the plate, the spray painting rate (at uniform speed) and the width of the overlapping area of the paint for each of the two spray strokes are obtained, which are *v* = 256.3 mm/s, *d* = 50.2 mm, respectively.

As shown in Figure 12, taking the experimental workpiece as an example, the workpiece can be regarded as a 3D entity. In the experiment, suppose the vertical distance from the gun to the workpiece surface is H, the bias algorithm can be used to obtain the spray path. Each spray parameter setting in the optimal algorithm of the spray painting trajectory are as follows: The ideal paint thickness is *qd* = 50 μm, the maximum allowable deviation of thickness is *qw* = ±10 μm, the spray radius is *R* = 60 mm, the spray distance is *H* = 100 mm, and the velocity of the spray is *v* = 256.3 mm/s. The workpiece surface is divided into five patches after the modeling work and the path at the junction of the two patches is the PA-PA mode. GA, ACO, and PSO are used to carry out experiments when solving the TTOI. The parameters of the GA algorithm are set as: Population size is *Psize* = 100, crossover probability is *xrate* = 0.20, mutation probability is *mrate* = 0.05, and the maximum number of evolutionary generation is *T* = 100. The parameters of the ACO algorithm are set as: The number of ants is *m* = 10, parameter *α* = 1, parameter *β* = 5, the volatilization rate of the pheromone is *ρ* = 0.5, constant *Q* = 100, and the maximum number of iterations is *Nmax* = 100. The parameters of the PSO algorithm are set as: The maximum speed threshold is *ε* = 1000, the number of particles is 20, learning factors are *c*1 = *c*2 = 2, and the maximum number of iterations is *Nmax* = 100.

**Figure 12.** The experimental workpiece.

The self-developed offline programming system of the spray painting robot is used for the spray test [20,21]. The part of the optimized trajectories of different patches on the workpiece surface are shown in Figure 13. The paint thicknesses of 400 discrete points are measured by the paint thickness gauge after the spray painting operation. The paint thickness at the sample point after spray painting in the optimized trajectory is shown in Figure 14, among which the maximum paint thickness is *qmax* = 55.6 μm, and the minimum paint thickness is *qmin* = 45.5 μm. It can be seen that all the paint thicknesses at the sampling point are within the maximum allowable deviation of the paint thickness *qw*, which is in line with the requirements of the spray quality.

**Figure 13.** (**a**) Optimized trajectory at the side of the patch; (**b**) optimized trajectory at the top of the patch.

**Figure 14.** The paint thickness at the sampling points.

From the point of view of spray painting efficiency, the compared results of GA, ACO, PSO, and random combination are shown in Table 1. It can be seen from the results that the total length of the spray trajectory using the PSO algorithm is the shortest, the spray painting time is the least, and the execution time of the system operation is the longest, which is within the allowable range in the practical application. For the workpiece, the spray painting time was reduced by 23% compared to a random combination. The total trajectories of GA and ACO are shorter than those of the random combination, and the spray painting time are reduced by 16% and 20%, respectively. It should be noted that the workpiece used here is only divided into five patches. For the workpiece, which is more complex and has more patches, the advantages of using the PSO algorithm in saving painting time will be more obvious, but the execution time of the offline programming system will be longer. Therefore, if the real-time performance of the system operations can meet the practical application requirements, the PSO algorithm will be the best choice. Otherwise, the GA algorithm or ACO algorithm can be taken into consideration.

**Table 1.** The compared results of different algorithms.

#### *5.6. Spray Painting Experiment*

As shown in Figures 15–18, taking the automotive body of a brand is as the paint objective. The thickness of the coating on each sampling point is shown in Figure 19. Each spray parameter settings in the optimal algorithm of the spray painting trajectory are as follows: The ideal paint thickness is *qd* = 50 μm, the maximum allowable deviation of the thickness is *qw* = ±10 μm, the spray radius is *R* = 60 mm, the spray distance is *H* = 80 mm, and the velocity of the spray is *v* = 389 mm/s. The left side of the car surface is divided into 15 patches and the trajectory of each patch is in a different

color. GA, ACO, and PSO are used to carry out experiments when solving the TTOI. The parameters of the GA algorithm are set as: Population size is *Psize* = 100, crossover probability is *xrate* = 0.20, mutation probability is *mrate* = 0.05, and the maximum number of evolutionary generation is *T* = 200. The parameters of the ACO algorithm are set as: The number of ants is *m* = 10, parameter *α* = 1, parameter *β* = 5, the volatilization rate of pheromone is *ρ* = 0.5, constant *Q* = 100, and the maximum number of iterations is *Nmax* = 200. The parameters of the PSO algorithm are set as: The maximum speed threshold is *ε* = 1000, the number of particles is 20, learning factors *c*1 = *c*2 = 2, and the maximum number of iterations is *Nmax* = 200.

**Figure 15.** An automobile paint line composed by several spray-painting robots.

**Figure 16.** Spray painting trajectory on the roof part of the automobile.

**Figure 17.** Spray painting trajectory on the left part and the left rear part of the automobile.

**Figure 18.** Spray painting trajectory on the right part and the right rear part of the automobile.

**Figure 19.** The paint thickness at the sampling points on the automobile body.

From the point of view of spray painting efficiency, the compared results of the GA, ACO, PSO, and random combination are shown in Table 2. It can be seen from the results that the total length of the spray trajectory using the PSO algorithm is the shortest, the spray painting time is the least, and the execution time of the system operation is the longest, which is within the allowable range in the practical application.

**Table 2.** The compared results of different algorithms.

#### **6. Conclusions**

In this paper, the spatial trajectory optimization method of a spray painting robot for 3D entity objects was proposed. Firstly, the finite range model of the paint deposition rate was established, and the 3D entity was sliced by the surface modeling method according to FPAG. Then, after planning the spray path on each patch, the variance of the paint thickness of the discrete point and the ideal paint thickness was taken as the objective function and the trajectory on each patch was optimized. The path at the junction of two patches was the PA-PA (parallel-parallel) mode. The improved GA algorithm, ACO algorithm, and PSO algorithm were used to solve the TTOI problem. The practicability of the algorithms was verified by simulation experiments. Finally, spraying experiments were conducted on the off-line programming experimental platform of the spraying robot, and the results of the three algorithms were studied. The results of the automobile body spraying experiments showed that the proposed trajectory optimization of the 3D entity spraying robot can completely satisfy the requirements of uniformity of the spraying thickness. More experimental data please see the Supplementary Materials.

**Supplementary Materials:** The following are available online at http://www.mdpi.com/2079-9292/8/1/74/s1.

**Author Contributions:** W.C. and Y.T. conceived and designed the experiments; W.C. performed the experiments; H.L. and J.L. analyzed the data; X.W. contributed reagents/materials/ analyzed the data; X.W. contributed reagents/materials/analysis tools; X.W. wrote the paper.

**Funding:** This research was supported by the National Natural Science Foundation of China under grant 61503162, "Six talent peaks" high-level talents projection Jiangsu Province under grant 2016GDZB021, Project funded by China Postdoctoral Science Foundation under grant 2016M601691, Industrial Foresight Project of Zhenjiang City under grant GY2018018.

**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* **Robust Visual Compass Using Hybrid Features for Indoor Environments**

**Ruibin Guo 1,\*, Keju Peng 1, Dongxiang Zhou 1 and Yunhui Liu 2**

Received: 15 December 2018; Accepted: 7 February 2019; Published: 16 February 2019

**Abstract:** Orientation estimation is a crucial part of robotics tasks such as motion control, autonomous navigation, and 3D mapping. In this paper, we propose a robust visual-based method to estimate robots' drift-free orientation with RGB-D cameras. First, we detect and track hybrid features (i.e., plane, line, and point) from color and depth images, which provides reliable constraints even in uncharacteristic environments with low texture or no consistent lines. Then, we construct a cost function based on these features and, by minimizing this function, we obtain the accurate rotation matrix of each captured frame with respect to its reference keyframe. Furthermore, we present a vanishing direction-estimation method to extract the Manhattan World (MW) axes; by aligning the current MW axes with the global MW axes, we refine the aforementioned rotation matrix of each keyframe and achieve drift-free orientation. Experiments on public RGB-D datasets demonstrate the robustness and accuracy of the proposed algorithm for orientation estimation. In addition, we have applied our proposed visual compass to pose estimation, and the evaluation on public sequences shows improved accuracy.

**Keywords:** visual compass; orientation estimation; hybrid features; plane tracking; vanishing direction; Manhattan World; RGB-D camera

#### **1. Introduction**

Robust orientation estimation is of great significance in robotics tasks such as motion control, autonomous navigation, and 3D mapping. Orientation can be obtained by utilizing carried sensors like the wheel encoder, inertial measurement unit (IMU) [1–4], or cameras [5–7]. Among these solutions, the visual-based method [8–11] is effective, as cameras can conveniently capture informative images to estimate orientation and position. In the past decades, many simultaneous localization and mapping (SLAM) systems [12,13] and visual odometry (VO) methods [14,15] have been proposed. Payá et al. [5] proposed a global description method based on Radon Transform to estimate robots' position and orientation with the equipped catadioptric vision sensor. These methods show good performance in estimating orientation from captured images. However, local and global maps' construction or loop detection is needed in these approaches to reduce drift error.

For most indoor environments, there exist many parallel and orthogonal lines and planes (called the Manhattan World (MW) [16]). These structural regularities are exploited in studies to estimate drift-free rotation without previous complex techniques (map reconstruction and loop closure) [17–19]. Since 3D geometric structures can easily be calculated by using the camera that provides both depth information and color image with 3 channels (red, green, and blue), called the RGB-D camera. The RGB-D camera has become a popular alternative to monocular and stereo cameras for the purpose

of rotation estimation, and estimation accuracy has been prominently improved by using the MW assumption with a RGB-D camera [20–23]. However, a major disadvantage of these MW-based methods is that the number of lines and planes used for tracking the MW axes must be no less than 2, which is the minimal sampling for 3 degrees of freedom (DoF). In practice, robots often encounter harsh environments without lines, and only one plane can be visible, resulting in failure in tracking MW axes to estimate the camera orientation. To address these issues, we select some frames as keyframes and exploit hybrid features (i.e., plane, line, and point) to compute the rotation matrix of each captured frame with respect to its reference keyframe instead of directly aligning with the global MW axes.

In this paper, we propose a robust and accurate approach for orientation estimation using RGB-D cameras. We detected and tracked the normal vectors of multiple planes from depth images, and we detected and matched the line and point features from color images. Then, by utilizing these hybrid features (i.e., plane, line, and point), we constructed a cost function to solve the rotation matrix of each captured frame. Meanwhile, we selected keyframes to reduce drift error and avoid directly aligning each frame with the global MW. Furthermore, we extracted the MW axes based on the normal vectors of orthogonal planes and the vanishing directions of parallel lines, and by aligning the current MW axes with the global MW axes, we refined the aforementioned rotation matrix of each keyframe and achieved drift-free orientation. Experiments showed that our proposed method produces lower drift error in a variety of indoor sequences compared to other state-of-the-art methods.

Our algorithm exploits hybrid features and adds a refinement step for keyframes, which can provide robust and accurate rotation estimation, even in harsh environments, as well as general indoor environments. The contributions of this work are as follows:

#### **2. Related Work**

Pose estimation obtained by VO or V-SLAM systems has been extensively studied for the purpose of meaningful applications, such as autonomous robots and augmented reality. Rotation estimation is usually considered as a subproblem of pose estimation that consists of rotational and translation components. It has been gradually recognized by researchers that the main source of VO drift is inaccurate rotation estimation [19,20]. In the following discussion, we focus on rotation-estimation methods that exploit structural regularities with RGB-D cameras. These methods utilize surface normals, vanishing points (vanishing directions), or mixed constraints to compute camera orientation.

Surface-normal vectors were exploited to estimate camera orientation because their distribution on the unit sphere (or called Gaussian sphere) is regular and more likely around plane-normal vectors in the current environment, as shown in Figure 1. The work of Straub et al. [21] introduced the Manhattan-Frame model in the surface-normal space and proposed a real-time maximum a posteriori (MAP) inference algorithm to estimate drift-free orientation. Zhou et al. [23] developed a mean-shift paradigm to extract and track planar modes in surface-normal vector distribution on the unit sphere, and achieved drift-free behavior by registering the bundle of planar modes. In the work of Kim et al. [24], orthogonal planar structures were exploited and tracked with an efficient SO(3)-constrained mean-shift algorithm to estimate drift-free rotation. These surface-normal-based methods can provide stable and accurate rotation estimation if the number of observed orthogonal planes is not less than two.

(**a**) RGB-D image pairs (**b**) 3D point cloud (**c**) Distribution of surface normals

**Figure 1.** Single RGB-D frame and distribution of surface normals corresponding to its 3D point cloud. (**a**) Frame 85 in 'LivingRoom0' sequence of ICL-NUIM dataset. (**b**) 3D point cloud obtained by back-projecting the depth information and coloring with aligned RGB pixels. (**c**) Distribution of surface normals on the unit sphere.

A vanishing point (VP) of a line is obtained by intersecting the image plane with a ray parallel to the world line and passing through the camera center, and it depends only on the direction of a line [25]. Two parallel lines determine a vanishing direction (VD), and the Euclidean 3D transformation of a VD is influenced only by rotation; the geometric relationships are shown in Figure 2, so VPs and VDs have been widely used for estimating rotation. Bazin et al. [17] proposed a three-line random-sample consensus (RANSAC) algorithm with the VP orthogonality constraint to estimate rotation. The work of Elloumi et al. [26] proposed a real-time pipeline for estimating camera orientation based on vanishing points for indoor navigation assistance on a smartphone. VP-based methods need a sufficient number of lines for estimating rotation, and accuracy performance is greatly affected by line-segment noise.

**Figure 2.** Three-dimensional geometric relationship between parallel lines and their vanishing direction. Gaussian sphere is a unit sphere on the center of a camera projection. Two parallel lines are projected onto the Gaussian sphere as two great circles, and vanishing direction is obtained by the cross project of these two great circles' normal vectors. Two parallel lines and their corresponding vanishing direction are drawn with red.

Hybrid approaches use both surface normals obtained in depth image and vanishing directions extracted in the RGB image to estimate rotation, which shows more robust performance. The method proposed by Kim et al. [22] exploited both line and plane primitives to deal with degenerate cases in surface-normal-based methods for stable and accurate zero-drift rotation estimation. In the work of Kim et al. [27], only a single line and a single plane in RANSAC were used to estimate camera orientation, and refinement is performed by minimizing the average orthogonal distance from the endpoints of the lines parallel to the MW axes once the initial rotation estimation is found. Bazin et al. [17] introduced a related one-line RANSAC for situations where the horizon plane is known.

#### **3. Proposed Method**

We propose a robust visual compass that exploits hybrid features (i.e., plane, line, and point) to estimate camera orientation with the RGB and depth-image pairs. Our proposed method has two main steps: (1) rotation matrix is estimated by tracking the hybrid features for each frame with respect to the reference keyframe (tracking step); and (2) refine the keyframe's initial rotation matrix by aligning with the global MW axes to achieve drift-free orientation (refinement step). The overview of our proposed method is shown in Figure 3.

**Figure 3.** Overview of our visual compass. We estimate camera rotation by tracking the plane, line, and point features. We refine the keyframe orientation by aligning the current Manhattan World (MW) axes with the global MW axes. Global MW axes is extracted from the first captured RGB-D frame.

#### *3.1. Rotation Estimation with Hybrid Features*

We simultaneously tracked multiple planes, lines, and points in the current environment, and we utilized tracked hybrid features to construct a cost function for estimating the current rotation matrix relative to its reference keyframe. This can provide camera rotation even in uncharacteristic scenes where there are no rich texture or no consistent visible lines.

#### 3.1.1. Multiple-Plane Detection and Tracking

We detected multiple planes from the depth image with a fast plane extraction algorithm [28]. The algorithm first constructs an initial graph that uniformly divides the depth image's point cloud into a set of nodes with size *H* × *W* in the image space. It then performs agglomerative hierarchical clustering on this graph to merge nodes belonging to the same plane. It final refines the extracted planes using pixel-wise region growing. With this approach, we obtain planes *Pi* : (**n***i*, *di*), *i* = 1, ..., *m*, where **n***i* is the unit normal vector of the *i*-th plane, and *di* is the distance to the origin of the camera co-ordinate system for the current frame.

We tracked the normal vector of each detected plane with a mean shift algorithm [23] that operates based on the density distribution of initial nodes' normals on the Gaussian sphere, as shown in Figure 4. We calculated the normal vector of each initial node by the least-square fitting method, and the depth image was preprocessed by a box filter to obtain the stable vectors. We used the previous frame's tracked (detected) normal vectors as an initial value, and then performed the mean shift algorithm in the tangent plane of the Gaussian sphere to obtain the tracked results. It should be noted that the parallel planes have the same plane normal vector; we class them as the same plane cluster for rotation estimation.

If we only have plane primitives to estimate rotation, rotation matrix *R* with three degrees of freedom can be computed from no less than two such tracked norm vectors that are not parallel because each normal vector represents two independent constraints on *R*.

$$\mathcal{R} = \arg\min\_{i=1,\ldots,m} \sum\_{i=1,\ldots,m} \left\| \mathbf{R} \cdot \mathbf{n}\_i^{ref} - \mathbf{n}\_i^k \right\|\_2^2 \tag{1}$$

where **n***ref i* represents the *i*-th detected plane normal vector in the reference keyframe, and **n***k i* represents the tracked result of the *i*-th plane in frame *k*.

**Figure 4.** Result of tracking planes: (**a**) Depth image of Frame 741 in the 'LivingRoom2' sequence of the ICL-NUIM dataset. (**b**) Tracked normal vectors from the Gaussian sphere; black dots represent the normals of initial nodes. (**c**) Plane primitives in current frame. Tacked normal vectors and their corresponding planes in image domain are represented with the same color.

#### 3.1.2. Line and Point Detection and Matching

We used a linear-time Line Segment Detector (LSD) [29] to extract 2D line segments on the color image and obtain 2D line segments set **l** = {*li*, *i* = 1, 2, ..., *n*}, where *li* is the *i*-th line segment: *y* = *kix* + *bi*, with starting point **u***si* = (*usi*, *vsi*) and ending point **u***ei* = (*uei*, *vei*). Pixels belonging to the line segment *li* are: ˜ = {**u**|**u** ∈ *li* ∧ **u** ∈ Ω}, Ω is the image domain. Then, we reconstructed their corresponding 3D points set: **P***l* = {**p**|**p** = *π*−1(**u**, *d*(**u**)), **p** ∈ *R*3 ∧ **u** ∈ }˜ , where *d*(**u**) represents the corresponding depth value of pixel **u** in the color domain, and *π*−1(**u**, *d*(**u**)) = *d*(**u**)( *u*−*cx fx* , *v*−*cy fy* , 1)*T* is the inverse projection function for a camera model, with *fx*, *fy* being the focal lengths on the *x* axis and *y* axis, and (*cx*, *cy*)*T* is the camera's centre co-ordinates. Finally, the RANSAC method is used to fit 3D lines *Li* : (**d***i*, **p***i*), where **d***i* represents the *i*-th line's 3D direction, and **p***i* represents a point in this 3D line.

We match the lines that were respectively extracted from the current frame and the reference keyframe based on the Line Band Descriptor (LBD) [30], and two pairs of matching lines that are not parallel are needed to estimate the rotation matrix in case that there are only line primitives obtained in the current environment.

$$\mathcal{R} = \arg\min\_{i=1\ldots n} \sum\_{i=1\ldots n} \left\| \mathbf{R} \cdot \mathbf{d}\_i^{ref} - \mathbf{d}\_i^k \right\|\_2^2 \tag{2}$$

where **d***ref i* represents the *i*-th detected 3D line direction in the reference keyframe, and **d***k i* represents the matching line direction in frame *k*.

In addition to the plane and line features, we extracted and matched the oriented fast and rotated brief (ORB) features for point tracking, as these features are extremely fast to compute and match, and they present good invariance to camera autogain, autoexposure, and illumination changes. We used the epipolar constraint method to optimize the initial matching pairs by ORB descriptors; the optimized results can provide reliable constraints to estimate the camera pose.

$$\mathcal{R} = \arg\min\_{i.\dots} \sum\_{i.\dots} \left\| \left( \mathbf{R} \cdot \boldsymbol{\pi}^{-1} \left( \mathbf{u}\_i^{\text{ref}}, d(\mathbf{u}\_i^{\text{ref}}) \right) + \mathbf{t} \right) - \boldsymbol{\pi}^{-1} (\mathbf{u}\_i^k, d(\mathbf{u}\_i^k)) \right\|\_{2}^2 \tag{3}$$

where **u***ref i* represents the 2D position of *i*-th detected ORB point feature in the reference keyframe and **u***k i* represents the 2D position of the matching point in frame *k*. It should be noted that translation component **t** could be obtained by solving Equation (3), but we did not consider it, as our visual compass mainly focused on camera orientation.

#### 3.1.3. Robust Rotation Estimation

We jointly utilized the tracked planes, lines, and points in the current environment to estimate the rotation matrix with respect to the reference keyframe. Rotation matrix *R* can be computed by solving:

$$\mathbf{R} = \arg\min\_{i=1\ldots m} \left\| \sum\_{i=1\ldots m} \lambda\_i^{\mathbb{P}} \left\| \mathbf{R} \cdot \mathbf{n}\_i^{\mathbb{P}f} - \mathbf{n}\_i^k \right\|\_2^2 + \sum\_{i=1\ldots n} \lambda\_i^L \left\| \mathbf{R} \cdot \mathbf{d}\_i^{\mathbb{P}f} - \mathbf{d}\_i^k \right\|\_2^2 + \sum\_{i=1\ldots N} \left\| \left( \mathbf{R} \cdot \mathbf{P}\_i^{\mathbb{P}f} + \mathbf{t} \right) - P\_l^k \right\|\_2^2 \tag{4}$$

where *λP i* represents the number of pixels contained in the *i*-th tracked plane, and *λL i* represents the number of pixels contained in the *i*-th tracked line, 3D points *Pref i* = *π*−1(**u***ref i* , *d*(**u***ref i* )), and *Pk i* = *π*−1(**u***k i* , *d*(**u***k i* )).

Cost function Equation (4) contains three parts, corresponding to plane, line, and point constraints. A stable and accurate rotation matrix can be solved by minimizing Equation (4) with line and plane constraints jointly in the texture-less environment that few points tracked, and the point constraints ensure that the rotation estimation is reliable in the scenes that no consistent lines or only one plane to be visible.

**Keyframe Selection:** By the tracking step, we constantly know the number of the tracked planes, lines and points for each frame. If there is only one tracked plane with the condition that the number of normal vectors on the Gaussian sphere around this tracked normal vector is too low, and the number of tracked points is less than a threshold, we reuse the fast plane extraction method and Line Segment Detector (LSD) method to detect planes and lines in the current frame. If the number of redetected orthogonal planes and lines is larger than 2, this frame is selected as a keyframe and performs the following refinement step.

#### *3.2. Drift-Free Orientation Estimation*

The previous tracking step estimates the rotation matrix between the current frame and its reference keyframe, and it is obvious that the accuracy of the reference keyframe's orientation directly affects the accuracy of the current frame's rotation matrix. To reduce drift error, we sought global MW axes in the first frame and refined each keyframe's orientation by aligning the current extracted MW axes with the global MW axes to achieve drift-free rotation in MW scenes. We sought current and global MW axes based on the plane normal vectors, and the vanishing directions of the parallel lines. Plane normal vectors can be directly obtained by the previous fast plane extraction method, and we propose a novel vanishing direction extraction method as follows.

#### 3.2.1. Vanishing Direction Extraction

To extract accurate VDs, we need to cluster lines that are parallel in the real world. We used the simplified Expectation–Maximization (EM) clustering method to group image lines and compute their corresponding 3D direction vectors. The original EM algorithm iterates the expectation and the maximization steps. In our simplified algorithm, we skip the expectation phase and roughly cluster the lines based on the K-means method [31], with the Euclidean distance of all extracted lines' 3D directions that are represented as 3D points. In the maximization phase, direction vectors are estimated by maximizing the objective function:

$$\mathbf{d}\_k^v = \arg\max\_i \prod\_i p(\mathbf{d}\_k^v | \mathbf{l}\_i^{(k)}) \tag{5}$$

where **l** (*k*) *i* represents the *i*-th 2D line segment in the *k*-th initial classification, **d***v k* represents the VD of the *k*-th initial classification to be optimized, and the *p*(**d***v k* |**l** (*k*) *i* ) represents the posterior likelihood of the VD.

Using the Bayes formula, the posterior likelihood of the VD is expressed as:

$$p(\mathbf{d}\_k^v | \mathbf{l}\_i^{(k)}) = \frac{p(\mathbf{l}\_i^{(k)} | \mathbf{d}\_k^v) p(\mathbf{d}\_k^v)}{p(\mathbf{l}\_i^{(k)})} \tag{6}$$

where *p*(**l** (*k*) *i* |**d***v k* ) represents the prior probability of the VD, and *p*(**d***v k* ) models the potential knowledge of the VD before we obtain the measurement. If we know nothing, *p*(**d***v k* ) is defined as uniform distribution with a constant value. Therefore, the VD can be obtained by maximizing prior probability:

$$\mathbf{d}\_{k}^{\upsilon} = \arg\max\_{i} \prod\_{i} p(\mathbf{d}\_{k}^{\upsilon} | \mathbf{l}\_{i}^{(k)}) = \arg\max\_{i} \sum\_{i} \log p(\mathbf{l}\_{i}^{(k)} | \mathbf{d}\_{k}^{\upsilon}) \tag{7}$$

Prior probability *p*(**l** (*k*) *i* |**d***v k* ) is defined as:

$$p(\mathbf{l}\_i^{(k)}|\mathbf{d}\_k^v) = \frac{1}{\sqrt{2\pi\sigma\_k^2}} (\frac{-(\mathbf{l}\_i^{(k)T}\mathbf{K}\mathbf{d}\_k^v)^2}{2\sigma\_k^2})\tag{8}$$

where **K** represents the internal camera parameters. Equation (8) reflects the fact that the vanishing direction is perpendicular to the plane normal of a great circle that is determined by an image line **l** (*k*) *i* and the center of the projection of the camera, as shown in Figure 2.

Maximizing objective function Equation (7) is equivalent to solving a weighted least-squares problem for each **d***v k* :

$$\mathbf{d}\_{k}^{v} = \arg\min\_{i} \sum\_{i} \frac{length(\mathbf{l}\_{i}^{(k)})}{\max(length(\mathbf{l}^{(k)}))} \cdot (\mathbf{l}\_{i}^{(k)\mathbf{T}} \mathbf{K} \mathbf{d}\_{k}^{v})^{2} \tag{9}$$

where *length*(**l** (*k*) *i* ) represents the length of the *i*-th line, and *max*(*length*(**l** (*k*))) represents the maximum line length in rough cluster *k*. Length coefficient is considered in the term because the longer the lines are, the more reliable they are. By solving Equation (9), we can obtain the initial vanishing direction and the residual for each line. To obtain a more accurate vanishing direction, we discarded lines with a larger residual than a threshold and added additional optimization. Optimized parallel lines are used to estimate the final vanishing directions. Figure 5 shows two results of parallel-line clustering obtained by our proposed method.

**Figure 5.** Results of parallel-line clustering that are used to compute vanishing directions: (**a**) the 120-th image in 'Living Room 2' sequence. (**b**) the 64-th image in 'Office Room 1' sequence. Lines with the same color are parallel in the real world. The vanishing directions obtained by these parallel lines can provide accurate and reliable constraints for MW extraction.

#### 3.2.2. Global Manhattan World Seeking

The Manhattan World assumption is suitable in human-made indoor environments, and we sought global MW axes based on plane normal vectors and the vanishing directions from the first frame. MW axes can be expressed as columns of a 3D rotation matrix *R* = [**r** *g* 1 **r** *g* 2 **r** *g* 3 ] ∈ *SO*(3).

We first set the the detected plane normal vectors and the vanishing directions from the parallel lines as the candidate MW axes, which return a redundant set. In fact, most of the pixels in the frame typically belong to the planes and lines that determine the dominant MW axes, and we sought the plane that contained the largest pixels, and set its plane normal vector **r**1 as the first MW axis. The remaining two axes **r**2 and **r**3 are determined based on the orthogonality constraint with the first axis and the number of the pixels belonging to the detected planes or the parallel lines.

If we detect three mutually orthogonal planes in the first frame, we directly set their plane-normal vectors as the MW axes. In the case that there are only two orthogonal planes, the third axis is determined by the vanishing direction from the parallel lines that is orthogonal with the two previous plane-normal vectors. Similarly, if only one plane is detected in the first frame, we sought the remaining two MW axes by the vanishing directions from the orthogonal parallel lines.

Initial MW axes [**r**1 **r**2 **r**3], obtained by the previous step, are not strictly orthogonal. We maintained orthogonality by reprojecting the MW axes onto the closest matrix on *SO*(3). Each axis is reweighted by a factor *λi* that is determined by the number of pixels belong to this axis' corresponding planes or parallel lines. The final global MW axes are obtained by using singular=value decomposition (SVD):

$$\mathbb{E}\left[\mathbf{r}\_1^{\mathcal{S}} \ \mathbf{r}\_2^{\mathcal{S}} \ \mathbf{r}\_3^{\mathcal{S}}\right] = \mathbf{U} \mathbf{V}^T \tag{10}$$

where [**U**, **D**, **V**] = *SVD*([*λ*1**r**1 *λ*2**r**2 *λ*3**r**3]) and factor *λi* describes how certain the observation of a direction is.

#### 3.2.3. Keyframe Orientation Refinement

For each keyframe, we refined its rotation matrix by aligning the current extracted MW axes with the global MW axes. We first used the fast plane-extraction algorithm and our proposed spatial-line direction estimation method to extract plane-normal vectors and vanishing directions in the current keyframe. We then extracted the current MW axes (**r***c i* , *i* = 1, 2, 3) by using the same method that we used to extract global MW axes. We finally determine the corresponding pairs based on the Euclidean distance between vector **r***c i* and **Rr***g j* , where **R** represents the rotation matrix obtained by tracking steps for the current keyframe. Refined rotation matrix **R***r* is computed by solving:

$$\mathbf{R}^r = \arg\min\_{i=1,2,3} \sum\_{i=1,2,3} \left\| \mathbf{R}^r \mathbf{r}\_i^S - \mathbf{r}\_i^c \right\|\_2^2 \tag{11}$$

where **r** *g i* and **r***c i* represent the *i*-th global MW axis and the current extracted corresponding MW axis.

#### **4. Results**

We evaluate our proposed approach on the synthetic dataset (ICL-NUIM [32]), real-world dataset (TUM RGB-D [33]), and pose-estimation application, respectively. All experiments were run on a desktop computer with an Intel Core i7, 16 GB memory, and Ubuntu 16.04 platform.

We compared our proposed approach with two state-of-the-art MW-based methods proposed by Zhou et al. [23] and Kim et al. [27], namely, orthogonal planes based rotation estimation (OPRE) and 1P1L. OPRE estimates absolute and drift-free rotation by exploiting orthogonal planes from depth images. 1P1L estimates 3DoF drift-free rotational motion with only a single line and plane in the Manhattan world. We used the average value of the absolute rotation error (ARE) in degrees as the performance metric for the entire sequences:

$$\text{AREAaverage} = \frac{1}{N} \sum\_{i=1}^{N} \arccos(\frac{tr(\mathbf{R}\_i^T \cdot \mathbf{R}\_i^{\mathcal{S}}) - 1}{2}) \times 57.3 \tag{12}$$

where *tr*() denotes the trace of a matrix, **R***i* and **R***g i* represent the estimated and ground rotation matrix for the *i*-th frame, respectively, and *N* represents the number of frames in the tested sequence.

#### *4.1. Evaluation on Synthetic Dataset*

We first tested the performance of our proposed algorithm on the ICL-NUIM dataset, and measured the average ARE in degrees for each sequence; evaluation results are shown in Table 1. The smallest average ARE values are bolded, which reveals that our proposed method is more accurate than the two other methods. For example, in 'Office Room 0', the average ARE of our proposed method is 0.16 degrees, while that of 1P1L and OPRE is 0.37 and 0.18 degrees, respectively. Our method outperformed the others in all cases in the ICL-NUIM benchmark. The main reason is that we jointly exploited the plane, line, and point features to estimate camera orientation even when the camera moves in scenes with no consistent lines, or where only one plane is visible; this is illustrated in Figure 6. In 'Living Room 0', the OPRE method failed to estimate the rotation for the entire sequence because only one plane can be visible in some frames; we marked the result as '×' in Table 1. The last column in Table 1 shows the number of frames in the current sequence.

**Figure 6.** Results of camera orientation estimated by our proposed method on the ICL-NUIM dataset: (**a**) 'Living Room 0' sequence. (**b**) 'Office Room 0' sequence. The estimated orientation of each frame is shown in the bottom of the RGB image. Colored thick and thin lines respectively denote current orientation and global MW axes (determined in the first frame); black lines represent ground orientation.

**Table 1.** Comparison of average absolute rotation error (ARE, degrees) on the ICL-NUIM dataset.

The MW assumption is sufficiently suitable for the ICL-NUIM benchmark and we used the refinement step to achieve a drift-free rotation matrix. To clearly show the effect of the refinement step, we measured the ARE values in degrees for all sequences by our method without a refinement step, which corresponds to the 'No Refinement' column in Table 1. We recorded the values of absolute rotation error (ARE) for each frame in the 'Living Room 0' sequence, and the final rotation drift with and without refinement was 0.34 and 1.43 degrees, respectively, as shown in Figure 7. This demonstrates that the refinement step can effectively reduce rotation drift.

**Figure 7.** Performance evaluation for refinement on the 'Living Room 0' sequence: (**a**–**c**) Roll, pitch, and yaw angles estimated by the proposed method with and without a refinement step for each frame. (**d**) Absolute rotation errors for our proposed methods with and without a refinement step. This shows that the refinement step can effectively reduce rotation drift.

#### *4.2. Evaluation on Real-World Data*